function [enhanced_speech] = v_spendred(input_speech,fs,algo_params)
%V_SPENDRED Speech Enhancement and Dereverberation by Doire
%
% Usage : (1)       [enhanced_speech] = v_spendred(corrupted_speech,fs)   % performs enhancement in order to denoise and dereverberate the input speech file
%         (2)       [enhanced_speech] = v_spendred(corrupted_speech,fs,algo_params)  % performs enhancement using the custom parameters specified in 'algo_params'
%
%
% Inputs:
%  input_speech     Noisy and reverberant input speech signal (single-channel)
%  fs               Sample frequency in Hz
%  algo_params      algorithm parameters [optional]
%
%
% Outputs:
%  enhanced_speech  Enhanced output speech file
%
% Algorithm Parameters:
%       The following parameters are defined in 'algo_params'. Default values are shown below.
%
%        algo_params.of=6                % Overlap factor [6]
%        algo_params.ti=0.005            % Desired frame increment in seconds [0.005]
%        algo_params.ri=1                % Set to 1 to round ti to the nearest power of 2 samples [1]
%        algo_params.sg=1                % Type of spectral subtraction gain to apply : 1=Wiener Gain, 2=Power Spectral Subtraction, 3=MMSE speech estimate [1]
%        algo_params.sc=0.95             % Smoothing constant for computation of the spectral gain [0.95]
%        algo_params.sf=1e-5;            % Floor for the spectral gain [1e-5]
%        algo_params.os=2;               % Interference Over-subtraction factor [2]
%        algo_params.cl=6;               % Number of HMM states to use (minimum is 2 - maximum is 6) [6]
% 		 algo_params.ds=1;          	 % Way of computing posterior distributions : 1 = max track , 2 = weighted sum of tracks
%        algo_params.mo=1;          	 % Mode : 'fast' = 1 or 'slow' = 0 [1]
%        algo_params.ef=-60;          	 % Energy floor (dB) [-60]
%
%
% References:
%
% [1] C. S. J. Doire, D. M. Brookes, P. A. Naylor, C. M. Hicks, D. Betts, M. A. Dmour, and S. H. Jensen.
%           Single-channel online enhancement of speech corrupted by reverberation and noise.
%           IEEE Trans. Audio, Speech, Language Processing, 25 (3): 572-587, Mar. 2017. doi: 10.1109/TASLP.2016.2641904.
% [2] B. Cauchi et al.,
%           Combination of MVDR beamforming and single-channel spectral processing for enhancing noisy and reverberant speech,
%           EURASIP J. Adv. Signal Process., vol. 61, 2015, pp. 1-12.
%
% Author :          Clement Doire
%                   clement.doire11@imperial.ac.uk
%
%
% Revision History:
%
%   0.5 - 16 Feb 2016  - First version, based on my PhD thesis and first draft of [1]
%   0.9 - 13 Mar 2016  - Several tweaks + improved overall performance
%   1.0 - 01 Jul 2016  - Base version used to generate results in the paper
%   1.1 - 19 Sep 2017  - Added algo_params.ef parameter to cope with zero input signals
%   1.2 - 22 Sep 2017  - Changed covariance matrix initialization to cope with zero input signals
%
% Comments:
%   - By default the algorithm is in 'fast' mode, which fails in rare
%       cases due to numerical errors. This can be overcome by using a Square Root implementation,
%       which is done via the 'slow' mode option algo_params.mo=0.
%

%      Copyright (C) Clement Doire 2016
%      Version: $Id: v_spendred.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%% TO DO %%%%
% (1) Deal with other sampling freqs more gracefully
% (2) add paramters for mu_mmse and beta_mmse; see [2] pp 5 & 7 for further information
fs_ori = fs; % save original sample frequency
if (fs ~= 16000)
    input_speech = resample(mean(input_speech,2),16000,fs);
    fs = 16000;
end
%%%% %%%%

if numel(input_speech)>length(input_speech)
    error('Input speech signal must be a vector, not a matrix !');
end
%Default algorithm constants
Csts.of=6;          % Overlap factor = (fft length)/(frame increment) [6]
Csts.ti=5e-3;       % Desired frame increment [0.005]
Csts.ri=1;          % Round ni to the nearest power of 2 [1]
Csts.sg=1;          % Type of spectral subtraction gain to apply : 1=Wiener Gain, 2=Power Spectral Subtraction, 3=MMSE speech estimate [1]
Csts.sc=0.95;       % Smoothing constant for computation of the spectral gain [0.95]
Csts.sf=1e-5;       % Floor for the spectral gain [1e-5]
Csts.os=2;          % Interference Over-subtraction factor [2]
Csts.cl=6;          % Number of HMM states to use (minimum is 2 - maximum is 6) [6]
Csts.ds=1;          % Way of computing posterior distributions : 1 = max track , 2 = weighted sum of tracks
Csts.mo=1;          % Mode : 'fast' = 1 or 'slow' = 0 [1]
Csts.ef=-60;        % Energy floor (dB) [-60]
if nargin>=3 && ~isempty(algo_params)
    qqn=fieldnames(Csts);
    for i=1:length(qqn)
        if isfield(algo_params,qqn{i})
            Csts.(qqn{i})=algo_params.(qqn{i});
        end
    end
end
%-------------------------------------------------------------------------%
%-------------------------------------------------------------------------%
%load the dictionary
mStates = [-28.4861773669176,-19.9241782718819,-16.4426351907036,...
    -13.2371952081660,-9.08277302468020,-7.94392920991245,-10.1807698459865,...
    -13.0242640127927,-15.0862478706129,-17.1550659704425,-20.5281316935197,-24.6762435506711,-28.8594807710477,-32.5786959842695,-34.7893823914205,...
    -36.3359261522268,-36.8192673337429,-36.4363892290271,-38.0294065203303,-42.0503710814368,-45.5963723705087,-48.2040932769337,-49.5383787887783,...
    -49.1147104408592,-48.8102903134417;-55.6639981588596,-61.5127504297246,-61.7239465777425,-62.8243042292255,-63.6413685588512,-63.4686731115175,...
    -63.7758454966487,-63.6874780526371,-63.8179939306919,-63.9503258211402,-63.7966111988872,-64.0463832623785,-64.0818520961556,-64.1192957981112,...
    -63.8411944409695,-63.6617787810601,-63.5628614840156,-63.1631597694168,-62.5434531164831,-61.9566668567832,-61.5323786575963,-61.3910567729765,...
    -61.1150168405862,-60.5161965622102,-59.9977551298916;-28.6934939143922,-19.2542393025089,-15.1143438538347,-12.7293198179762,-12.1005903838332,...
    -16.5017610503493,-21.3970568254212,-25.1613543930201,-28.8424911990220,-31.5618412114192,-32.1889756387523,-29.6313245488876,-26.0094143727967,...
    -23.9888695941115,-23.0689428325671,-23.6462241160502,-26.4296918344984,-27.1090165222226,-27.8756237542369,-31.1075250195298,-36.3923980939477,...
    -40.8889238387664,-43.4857021995535,-43.3831690409702,-42.9845505669993;-45.3678779268938,-43.6673845369666,-41.8441402268909,-39.9311165545071,...
    -37.8535557910413,-36.8469454100004,-36.6393657864992,-36.5362981350264,-36.9937748890843,-37.4953438142185,-37.0039280540463,-35.1723035151364,...
    -33.6115205373159,-33.4564666785186,-32.0578128508397,-29.7739710941898,-28.4547720864261,-25.9299736823261,-23.4486089524545,-21.9613450596309,...
    -21.4217191910235,-22.0225885633808,-23.1188663976374,-23.7853960376589,-24.1662738392603;-34.1432887176235,-26.3187620722736,-23.6590932454401,...
    -22.8473441058075,-23.1748279271661,-26.4411088266502,-29.2521006269876,-31.1772170063613,-33.6716054384278,-36.4153183945587,-38.6110013517002,...
    -39.9143397342699,-41.0618568585801,-41.5304011208901,-41.0824724701060,-42.0444890310335,-44.6508339686914,-45.7999391511713,-45.7777619091791,...
    -46.4288731995784,-48.2850680725583,-50.4157570788352,-51.6879601727512,-51.5283308152838,-51.3663633824326;-27.8144244788325,-17.3617936140541,...
    -13.3123479510278,-11.0494334946260,-6.50571075754786,-3.54362892667645,-4.20504911233702,-8.05475391358820,-11.8924915327701,-14.0090223475965,...
    -14.4546676449037,-14.2927721880105,-15.9448322180079,-18.7341480479014,-19.5966571286418,-20.5315589196482,-22.8229586537310,-23.3910949080587,...
    -24.7888227173666,-29.0646718145073,-33.6969302543408,-37.4939369422004,-40.2366579607097,-40.9400771136199,-41.1427749674294]';
covStates=zeros(25,25,6);
covStates(:,:,1)=[55.5505522382743,16.5334640886389,10.9178210580786,13.5974699822659,9.99267788404160,4.65546050187628,-2.81201462169877,-4.68388254847409,-1.74070984578874,...
    0.532309021380460,0.520434337039108,0.426187651661797,0.204973461632197,2.82145438186938,5.97778821497629,5.67116890799604,6.23627453582871,9.10204084709650,-1.40798388710344,-9.27841117298606,-8.67918146231544,-3.25690617757991,8.95727598557277,10.6180838403128,2.39233816833827;16.5334640886389,18.2352850499025,14.2982190621525,12.9147652127658,...
    10.9068883279432,5.09619622536809,1.99699567951996,1.77114097312344,2.10855628245821,2.21550979726713,1.99198106580820,1.60549252666784,2.11816018706768,2.39431959805263,1.74306080464593,1.55659212977964,2.61868440062802,2.85542520831475,2.32018239730699,2.21287283794584,2.02202932945204,3.12385163781442,5.52579633296699,6.64558367809775,...
    6.96332487679765;10.9178210580786,14.2982190621525,20.1762546173859,16.4595133663636,14.8609481425796,5.97970590427510,0.591906404060290,0.982978882864801,2.23925097828404,2.30331161085411,1.79452144128718,1.44667191507181,2.36522922281405,2.57029564268839,1.11330362916639,0.564044453802269,3.04280360620582,4.57266397236781,3.26155863446703,...
    1.92374058981673,0.245560805156978,1.55444787790639,4.51858938226580,6.33344817664139,6.62700755938737;13.5974699822659,12.9147652127658,16.4595133663636,25.1499859878925,20.9717174671958,8.03086063725880,-2.11790484496031,-1.37239240706099,0.715670186205292,1.08369420585053,1.08318667616261,1.64087296402881,2.76079382652549,1.47558313336968,...
    -1.75810347660927,-2.40899670388149,1.33147165170225,3.38102633874749,2.04167139593248,0.313900322666336,-3.12313780280289,-2.79114286038283,1.06703715938550,3.64389690205945,4.17640830369928;9.99267788404160,10.9068883279432,14.8609481425796,20.9717174671958,30.7321512531260,18.5741177763561,5.20554903668077,3.56026395652826,6.30578953991209,...
    5.07481582114155,1.57988987546524,0.351795912501729,1.66931379440034,1.05094554656604,-1.05941242887155,-1.70405300192755,2.34975400732156,5.76466601347604,3.49586289966016,0.582881393693762,-2.65434624191792,-3.78563269745683,-0.645391290118053,1.91540664601044,1.69556520737948;4.65546050187628,5.09619622536809,5.97970590427510,8.03086063725880,...
    18.5741177763561,32.8716134447691,27.6428223262770,22.6106228222990,21.8937639735911,16.8598648015587,5.86190237340298,-1.25506206170561,-0.936922116724214,2.53745873288493,5.78232195722709,6.24075811184744,6.63128488489175,9.09153262049025,7.61191175409975,4.16752822310257,4.61092868306428,3.57840885542566,3.08339626198595,2.77065429967224,...
    1.31764432785799;-2.81201462169877,1.99699567951996,0.591906404060290,-2.11790484496031,5.20554903668077,27.6428223262770,44.6997035514628,40.5836947361391,35.6346329460388,24.8919310834979,7.29727103163038,-5.08674449271691,-5.79065223491830,1.13130891841562,9.28608855604750,11.9599342837549,10.0040941367853,10.3032246837205,10.6528719283557,...
    7.97989032997790,10.6616729805806,11.3253682718420,7.92354036323794,5.24732304120822,4.15478163811481;-4.68388254847409,1.77114097312344,0.982978882864801,-1.37239240706099,3.56026395652826,22.6106228222990,40.5836947361391,50.0515110111500,45.0408295324871,26.3468878416713,0.455288753487265,-15.0942579478821,-15.4028656508688,-6.47079820860882,...
    5.00543109795995,11.0960536306407,11.1342737564266,10.9557815157042,11.1232688117597,8.51832407216314,10.4012401376221,12.3793207921021,10.0090373307991,7.45031848452570,6.18418395773286;-1.74070984578874,2.10855628245821,2.23925097828404,0.715670186205292,6.30578953991209,21.8937639735911,35.6346329460388,45.0408295324871,55.4002707137703,...
    40.9927419515717,5.64313689579180,-16.3365142731531,-17.5413602589661,-9.53214104224454,1.08844150224375,7.61647008781981,10.1852849741109,11.2547089880277,9.70128880044374,5.90965257043572,7.43430717651063,10.2328142443249,9.85723490408961,7.98719422386298,6.17187704200031;0.532309021380460,2.21550979726713,2.30331161085411,1.08369420585053,...
    5.07481582114155,16.8598648015587,24.8919310834979,26.3468878416713,40.9927419515717,60.0538337884872,42.2208293482854,15.3835685486830,7.49584063132029,3.04421940859051,-0.0246940339037096,0.920210707501100,4.81451664419560,5.97964019881028,4.29452064022631,1.32422647813735,2.27663234926819,4.15082834672372,5.52463127008954,5.33225618475827,...
    5.47911427785979;0.520434337039108,1.99198106580820,1.79452144128718,1.08318667616261,1.57988987546524,5.86190237340298,7.29727103163038,0.455288753487265,5.64313689579180,42.2208293482854,75.7969170856392,67.9205826904135,52.3364620497711,29.9798063791398,5.47010669783019,-3.66051757895065,-1.26757149309171,-1.64138758774107,-2.23111852810488,...
    -2.92201773783040,-3.91849417611373,-4.20473376382332,-1.67993711524748,0.249583209013453,4.03119700415693;0.426187651661797,1.60549252666784,1.44667191507181,1.64087296402881,0.351795912501729,-1.25506206170561,-5.08674449271691,-15.0942579478821,-16.3365142731531,15.3835685486830,67.9205826904135,96.5390650422304,86.5140267162725,51.7871230233052,...
    12.1810092023456,-5.58163137501841,-5.90675695570028,-7.88118963454921,-7.80029755162057,-6.42945146751197,-8.03142432980626,-9.67060427505694,-6.76129968629389,-3.61941224786485,2.14491521683075;0.204973461632197,2.11816018706768,2.36522922281405,2.76079382652549,1.66931379440034,-0.936922116724214,-5.79065223491830,-15.4028656508688,-17.5413602589661,...
    7.49584063132029,52.3364620497711,86.5140267162725,96.6601441442137,67.1947062155571,19.2444700822521,-4.16723188493323,-4.68341271878820,-8.13117076608092,-9.98602220149892,-8.54385525607331,-10.0352935484860,-12.2641540970264,-9.01859861712797,-5.13574811015694,1.09183917418801;2.82145438186938,2.39431959805263,2.57029564268839,1.47558313336968,...
    1.05094554656604,2.53745873288493,1.13130891841562,-6.47079820860882,-9.53214104224454,3.04421940859051,29.9798063791398,51.7871230233052,67.1947062155571,75.1096356216366,44.8926940568457,12.9841633617957,6.89317497837785,3.66714311683161,-2.07979367608287,-4.99476818269192,-5.89732391005612,-7.67923202972843,-5.74889951113904,-2.75987034199587,0.563303494087326;...
    5.97778821497629,1.74306080464593,1.11330362916639,-1.75810347660927,-1.05941242887155,5.78232195722709,9.28608855604750,5.00543109795995,1.08844150224375,-0.0246940339037096,5.47010669783019,12.1810092023456,19.2444700822521,44.8926940568457,66.8238836138358,47.0670585999202,24.4471095918320,19.9590855590048,13.8657652546224,5.45566978995291,4.20629674408597,3.69089538710752,...
    3.16485268441084,3.61720907068856,1.72910830737191;5.67116890799604,1.55659212977964,0.564044453802269,-2.40899670388149,-1.70405300192755,6.24075811184744,11.9599342837549,11.0960536306407,7.61647008781981,0.920210707501100,-3.66051757895065,-5.58163137501841,-4.16723188493323,12.9841633617957,47.0670585999202,68.6471027986508,47.5565924311593,29.4632163050920,23.1072350329729,...
    13.3177220760717,9.69285629180968,9.81618652827382,8.60031828403043,7.95951420468698,5.14709757425915;6.23627453582871,2.61868440062802,3.04280360620582,1.33147165170225,2.34975400732156,6.63128488489175,10.0040941367853,11.1342737564266,10.1852849741109,4.81451664419560,-1.26757149309171,-5.90675695570028,-4.68341271878820,6.89317497837785,24.4471095918320,47.5565924311593,...
    62.6938106903565,45.1846268416747,24.4965005954548,12.5520203839324,5.30014242209754,4.77357628356460,5.97526653915916,6.31994386155144,5.11979594194596;9.10204084709650,2.85542520831475,4.57266397236781,3.38102633874749,5.76466601347604,9.09153262049025,10.3032246837205,10.9557815157042,11.2547089880277,5.97964019881028,-1.64138758774107,-7.88118963454921,-8.13117076608092,...
    3.66714311683161,19.9590855590048,29.4632163050920,45.1846268416747,64.7489573744218,45.0689147928324,19.2427366651555,8.33692844262386,3.59961885659603,4.59046266200724,5.47249086568356,3.45463693142211;-1.40798388710344,2.32018239730699,3.26155863446703,2.04167139593248,3.49586289966016,7.61191175409975,10.6528719283557,11.1232688117597,9.70128880044374,4.29452064022631,-2.23111852810488,...
    -7.80029755162057,-9.98602220149892,-2.07979367608287,13.8657652546224,23.1072350329729,24.4965005954548,45.0689147928324,64.1301804054733,43.2560356986102,21.5269055163974,9.68052048973873,3.41378146579554,4.95439785100326,6.50368554638616;-9.27841117298606,2.21287283794584,1.92374058981673,0.313900322666336,0.582881393693762,4.16752822310257,7.97989032997790,8.51832407216314,5.90965257043572,1.32422647813735,...
    -2.92201773783040,-6.42945146751197,-8.54385525607331,-4.99476818269192,5.45566978995291,13.3177220760717,12.5520203839324,19.2427366651555,43.2560356986102,65.7018262312750,44.9219016699696,20.6991916302035,5.95401370807739,5.71086904687721,9.82887819977783;-8.67918146231544,2.02202932945204,0.245560805156978,-3.12313780280289,-2.65434624191792,4.61092868306428,10.6616729805806,10.4012401376221,...
    7.43430717651063,2.27663234926819,-3.91849417611373,-8.03142432980626,-10.0352935484860,-5.89732391005612,4.20629674408597,9.69285629180968,5.30014242209754,8.33692844262386,21.5269055163974,44.9219016699696,67.4258426552536,45.2811192881963,16.3698193996470,7.44618720466919,9.63373383196222;-3.25690617757991,3.12385163781442,1.55444787790639,-2.79114286038283,-3.78563269745683,3.57840885542566,...
    11.3253682718420,12.3793207921021,10.2328142443249,4.15082834672372,-4.20473376382332,-9.67060427505694,-12.2641540970264,-7.67923202972843,3.69089538710752,9.81618652827382,4.77357628356460,3.59961885659603,9.68052048973873,20.6991916302035,45.2811192881963,67.8938203359553,42.9113247397312,17.9492567590164,11.4207436607287;8.95727598557277,5.52579633296699,4.51858938226580,1.06703715938550,...
    -0.645391290118053,3.08339626198595,7.92354036323794,10.0090373307991,9.85723490408961,5.52463127008954,-1.67993711524748,-6.76129968629389,-9.01859861712797,-5.74889951113904,3.16485268441084,8.60031828403043,5.97526653915916,4.59046266200724,3.41378146579554,5.95401370807739,16.3698193996470,42.9113247397312,62.6422990212510,39.6458841134889,18.4032107478800;10.6180838403128,6.64558367809775,...
    6.33344817664139,3.64389690205945,1.91540664601044,2.77065429967224,5.24732304120822,7.45031848452570,7.98719422386298,5.33225618475827,0.249583209013453,-3.61941224786485,-5.13574811015694,-2.75987034199587,3.61720907068856,7.95951420468698,6.31994386155144,5.47249086568356,4.95439785100326,5.71086904687721,7.44618720466919,17.9492567590164,39.6458841134889,53.3366237959440,34.0847934778521;...
    2.39233816833827,6.96332487679765,6.62700755938737,4.17640830369928,1.69556520737948,1.31764432785799,4.15478163811481,6.18418395773286,6.17187704200031,5.47911427785979,4.03119700415693,2.14491521683075,1.09183917418801,0.563303494087326,1.72910830737191,5.14709757425915,5.11979594194596,3.45463693142211,6.50368554638616,9.82887819977783,9.63373383196222,11.4207436607287,18.4032107478800,34.0847934778521,47.5182865309538];
covStates(:,:,2)=[42.2542681718899,33.1398832534293,26.9122779284013,21.9440282770309,16.2682055376897,11.2120751265590,9.89236392704602,9.09916342753589,8.21588946832406,8.12635763388025,7.60734158280344,8.00270814218398,8.20752942975105,8.32179207688550,8.52556355020916,8.40985634858555,8.05221738526830,7.95277955181197,8.03273914881971,8.17370504867544,8.41530644706598,9.05017983666968,10.0267588624726,11.0560751227524,11.6396525878338;33.1398832534293,64.4820752208886,59.0278742927478,46.2378954712472,32.0083315372066,21.1263008696744,17.2015759944404,14.3839235482175,12.0985187658515,10.6544391605358,9.75603750747912,10.2647369507516,9.78697212706562,9.56716799102638,9.68730159111792,8.85687366696022,7.37122945360696,6.46235707299659,6.31892781267847,6.25086081523959,6.13999254370508,5.89195327667544,5.92476618053937,6.33925055227010,6.50907903067561;26.9122779284013,59.0278742927478,68.7990622998320,55.7602063536266,36.4028926934798,24.6716152937138,20.4536836879006,17.3864203791402,14.6169240593445,12.5016575669034,11.4894711110888,12.0382417381908,11.1867514784698,10.5746182460189,10.5286312517389,9.37636846920350,7.47862062185348,6.33788982150573,6.04918759142116,5.85887204591981,5.53005641219455,5.00152887857997,4.70379791726771,4.80651047077985,4.73884419082275;21.9440282770309,46.2378954712472,55.7602063536266,57.3683453014700,40.7416179979095,27.4784734437890,23.5823275744409,20.8691132413932,...
    17.7448231557547,14.9503498825179,13.7520420147822,14.2460844688198,13.2930904266261,12.3149825653345,11.9174243871468,10.6004447715276,8.63187339323386,7.48380655381254,7.03731382226207,6.91810619761794,6.58527758084905,5.78615681667555,5.12760211779750,5.04412620584148,4.83947369580497;16.2682055376897,32.0083315372066,36.4028926934798,40.7416179979095,44.1658899430629,32.7470258174578,26.3733792890405,23.6172297172381,20.3633012419433,17.2560879334150,16.0174733511536,16.3445897219769,15.4362285720737,14.0870247705533,13.2484590643142,11.9576345058210,10.3446152540345,9.34568716822910,8.87609884011808,8.75444633909608,8.42313492297138,7.51927133497993,6.70065601879916,6.49587145056591,6.21252000108338;11.2120751265590,21.1263008696744,24.6716152937138,27.4784734437890,32.7470258174578,36.5247514484939,30.8043850884372,25.2497575113821,21.8259778893446,18.5115656145718,17.2727589851859,17.4814192671050,16.4067764329033,14.6685822908654,13.3789700856198,12.1929148152552,11.0560533955463,10.2353314726676,9.79281951299369,...
    9.50498773282756,9.19895612560047,8.37378868466334,7.57650526892307,7.22627713594640,6.92510618914010;9.89236392704602,17.2015759944404,20.4536836879006,23.5823275744409,26.3733792890405,30.8043850884372,37.7027006822321,31.7875736162849,25.0778722555869,21.1206727963219,19.6925651861192,19.4404146613118,17.8524909464742,15.7907307231729,14.3153877467844,12.9688009984997,11.8132191118816,11.0621435772935,10.6517301026570,10.4502680328554,10.1272893283078,9.19064782877994,...
    8.29394385936114,7.89201254869522,7.57090728115097;9.09916342753589,14.3839235482175,17.3864203791402,20.8691132413932,23.6172297172381,25.2497575113821,31.7875736162849,38.2149076276160,30.1508330876474,23.2088091210994,21.1799175431942,20.4845661525898,18.5764914692363,16.2904151034854,14.7990177723974,13.4448221695129,12.3769282510949,11.6894517093901,11.2902852046053,11.2307297226901,10.9618665008475,9.93893597236234,8.99000014125696,8.60527932539123,8.20779058495423;8.21588946832406,12.0985187658515,14.6169240593445,17.7448231557547,20.3633012419433,21.8259778893446,25.0778722555869,30.1508330876474,33.4759776220794,25.9023830924271,21.1453643537086,19.6890177460098,17.9752318385214,15.9942538134494,14.6425432659277,13.3677630310837,12.5846445121367,12.0496671565259,11.7206100355555,11.7724536209992,11.4943659341896,10.5978078950192,9.72093937280370,9.33521113520222,8.97988361999252;8.12635763388025,10.6544391605358,12.5016575669034,14.9503498825179,17.2560879334150,18.5115656145718,21.1206727963219,23.2088091210994,...
    25.9023830924271,31.8069570542182,25.9051378207540,19.5120672142656,17.2127609678812,15.6034230249996,14.6618009918470,13.6903914235496,12.9148255930402,12.5194665341208,12.3956575477847,12.5706063937445,12.2444214701460,11.3972408694496,10.5640834177449,10.3008301307204,9.95843014528928;7.60734158280344,9.75603750747912,11.4894711110888,13.7520420147822,16.0174733511536,17.2727589851859,...
    19.6925651861192,21.1799175431942,21.1453643537086,25.9051378207540,30.5674605936240,23.3963044942150,18.2871258593719,15.7304677796722,14.8356360738005,14.0187901544858,13.1921488286600,12.8916378219120,12.8300864768943,12.8054869586340,12.4581422093284,11.6507818422982,10.8618033926377,10.5842056402214,10.2848528160644;8.00270814218398,10.2647369507516,12.0382417381908,14.2460844688198,16.3445897219769,17.4814192671050,19.4404146613118,20.4845661525898,19.6890177460098,19.5120672142656,23.3963044942150,28.7189469271733,22.7382704140106,17.3374674310259,15.6978780957798,14.6147785629468,13.7430227503510,13.4165282553592,13.2501896723629,13.0165342073952,12.5807709755632,11.8388536438749,10.9878533831768,10.5338734452740,10.3436747319479;8.20752942975105,9.78697212706562,11.1867514784698,13.2930904266261,15.4362285720737,16.4067764329033,17.8524909464742,18.5764914692363,17.9752318385214,17.2127609678812,18.2871258593719,22.7382704140106,26.9281693632404,20.9676530692716,17.0267793246475,15.8109367808409,14.9096130218979,...
    14.4477446657288,14.2507009532318,13.9593442224356,13.5163861004693,12.6957325913323,11.7989986083618,11.3818436606364,11.1892480288838;8.32179207688550,9.56716799102638,10.5746182460189,12.3149825653345,14.0870247705533,14.6685822908654,15.7907307231729,16.2904151034854,15.9942538134494,15.6034230249996,15.7304677796722,17.3374674310259,20.9676530692716,24.7208888366138,20.1809079483132,17.0661384382632,16.0250670334491,15.4487117071041,15.4242252273914,15.5033870812895,15.2243844454306,14.2369666440276,13.2855466217531,12.9572570924219,12.6957485467655;8.52556355020916,9.68730159111792,10.5286312517389,11.9174243871468,13.2484590643142,13.3789700856198,14.3153877467844,14.7990177723974,14.6425432659277,14.6618009918470,14.8356360738005,15.6978780957798,17.0267793246475,20.1809079483132,24.0686273777599,19.7947291240874,16.7816935536301,16.0312192142737,16.0129522231429,16.2700222883411,16.0951138243464,15.0390556694171,14.0436854665189,13.7475553012914,13.4180426534560;8.40985634858555,8.85687366696022,9.37636846920350,10.6004447715276,11.9576345058210,12.1929148152552,12.9688009984997,13.4448221695129,13.3677630310837,13.6903914235496,14.0187901544858,14.6147785629468,15.8109367808409,17.0661384382632,19.7947291240874,23.0315801126879,18.9109961908672,16.6459345560997,16.5381880147804,16.7191961501820,16.4934788217381,15.6121779481512,14.6348508116704,14.2726263707707,13.9753795232301;8.05221738526830,...
    7.37122945360696,7.47862062185348,8.63187339323386,10.3446152540345,11.0560533955463,11.8132191118816,12.3769282510949,12.5846445121367,12.9148255930402,13.1921488286600,13.7430227503510,14.9096130218979,16.0250670334491,16.7816935536301,18.9109961908672,21.7268549812357,18.6803494348202,17.3047313164992,17.2871346213239,16.9417305361393,16.1642010753901,15.2678075792088,14.9240833604240,14.7336289117962;7.95277955181197,6.46235707299659,6.33788982150573,7.48380655381254,9.34568716822910,10.2353314726676,11.0621435772935,11.6894517093901,12.0496671565259,12.5194665341208,12.8916378219120,13.4165282553592,14.4477446657288,15.4487117071041,16.0312192142737,16.6459345560997,18.6803494348202,21.8698646259090,19.4948598845312,18.2166407206852,17.7065159898375,16.8244160509924,16.0516809602189,15.7429250755869,15.5323347628298;8.03273914881971,6.31892781267847,6.04918759142116,7.03731382226207,8.87609884011808,9.79281951299369,10.6517301026570,11.2902852046053,11.7206100355555,12.3956575477847,12.8300864768943,13.2501896723629,...
    14.2507009532318,15.4242252273914,16.0129522231429,16.5381880147804,17.3047313164992,19.4948598845312,22.9539889998102,20.9904348585530,19.3516745348877,18.0879518676471,17.1313274417101,16.7367874252775,16.5140470876328;8.17370504867544,6.25086081523959,5.85887204591981,6.91810619761794,8.75444633909608,9.50498773282756,10.4502680328554,11.2307297226901,11.7724536209992,12.5706063937445,12.8054869586340,13.0165342073952,13.9593442224356,15.5033870812895,16.2700222883411,16.7191961501820,17.2871346213239,18.2166407206852,20.9904348585530,24.6340142427769,22.4388595014550,19.8313480597759,18.4710136661618,18.0263435663104,17.6966228995057;8.41530644706598,6.13999254370508,5.53005641219455,6.58527758084905,8.42313492297138,9.19895612560047,10.1272893283078,10.9618665008475,11.4943659341896,12.2444214701460,12.4581422093284,12.5807709755632,13.5163861004693,15.2243844454306,16.0951138243464,16.4934788217381,16.9417305361393,17.7065159898375,19.3516745348877,22.4388595014550,25.5813598636473,22.2770770284096,19.7528707240743,19.2187340996958,18.7904680191854;9.05017983666968,5.89195327667544,5.00152887857997,5.78615681667555,7.51927133497993,8.37378868466334,9.19064782877994,9.93893597236234,10.5978078950192,11.3972408694496,11.6507818422982,11.8388536438749,12.6957325913323,14.2369666440276,15.0390556694171,15.6121779481512,16.1642010753901,16.8244160509924,18.0879518676471,19.8313480597759,22.2770770284096,...
    23.9427201699074,21.0997331299115,19.7986204829634,19.3290497419985;10.0267588624726,5.92476618053937,4.70379791726771,5.12760211779750,6.70065601879916,7.57650526892307,8.29394385936114,8.99000014125696,9.72093937280370,10.5640834177449,10.8618033926377,10.9878533831768,11.7989986083618,13.2855466217531,14.0436854665189,14.6348508116704,15.2678075792088,16.0516809602189,17.1313274417101,18.4710136661618,19.7528707240743,21.0997331299115,22.7169795756540,20.9985740856786,19.9515015945520;11.0560751227524,6.33925055227010,4.80651047077985,5.04412620584148,6.49587145056591,7.22627713594640,7.89201254869522,8.60527932539123,9.33521113520222,10.3008301307204,10.5842056402214,10.5338734452740,11.3818436606364,12.9572570924219,13.7475553012914,14.2726263707707,14.9240833604240,15.7429250755869,16.7367874252775,18.0263435663104,19.2187340996958,19.7986204829634,20.9985740856786,22.9842650551391,21.4695001191088;11.6396525878338,6.50907903067561,4.73884419082275,4.83947369580497,6.21252000108338,6.92510618914010,7.57090728115097,...
    8.20779058495423,8.97988361999252,9.95843014528928,10.2848528160644,10.3436747319479,11.1892480288838,12.6957485467655,13.4180426534560,13.9753795232301,14.7336289117962,15.5323347628298,16.5140470876328,17.6966228995057,18.7904680191854,19.3290497419985,19.9515015945520,21.4695001191088,23.2336372627411];
covStates(:,:,3)=[68.6217460907900,21.1676812682065,11.7328970617784,17.0370917487721,5.38753694153238,-7.46678251410560,-4.25370721254760,-4.66530692581079,-2.51727840325899,-0.0222693428525085,7.77271764064578,15.8892570422519,20.2557909574935,15.0499455736291,6.71439781940967,-0.215644526778094,-7.53674644535830,6.37908866685120,7.58491557425687,-12.4662256646260,-20.3334206261375,-11.0202990043130,5.31395653148383,9.07737891954220,2.82651770353372;21.1676812682065,28.0686481419400,24.6762141586202,22.3891625703721,19.0155999152911,8.56822989981309,4.62566966250763,2.84351679390897,1.09976231738547,-0.430958411928135,-1.66921241769178,-0.921167853845433,2.13736114488578,5.79125843861373,6.89947737024852,6.20779611628890,5.01156071334007,5.23163176987512,5.21605272525785,1.85978240005883,-1.98976643262044,-1.83352733616632,0.200381018506610,1.78445605294837,2.64615485644659;11.7328970617784,24.6762141586202,37.0442508880661,31.6858279646658,23.3798542995236,11.2043255855097,4.01332627819630,1.75273592864460,0.308034614343833,-0.748387958340454,-2.12193478804064,-1.20656809658165,3.80442620051941,10.3758286758611,12.2871479790389,11.0248476286196,10.1626689024442,11.8755984854200,9.99589074642841,3.45373914111269,-3.60052534762597,-4.17205663582501,-2.00656654985110,0.190204427590809,1.59433432378998;17.0370917487721,22.3891625703721,31.6858279646658,44.4704692197245,34.1739358955294,15.2654942328664,...
    6.79294390493924,2.67066518158898,0.511959074292315,0.145692285708609,0.717149484132317,3.72055225536004,9.80684013028351,14.8095004677359,14.9861322117183,12.0969247937467,9.97444495277138,12.1336595171325,10.7689256498738,2.81499883719888,-5.95682083402728,-7.54682005594304,-5.30507835073042,-3.39487727855270,-2.12888973859252;5.38753694153238,19.0155999152911,23.3798542995236,34.1739358955294,48.2277593683994,33.4738990907830,19.1783116705422,12.2583479560372,8.40388068523053,6.38586268081686,5.87521395197408,8.76329751567099,12.4345759540831,11.9812162569625,12.0062466666891,11.7524734022862,9.75492382009717,7.72249685023660,7.37892285808970,5.70061848255443,0.995525690391575,-4.40395983807157,-6.71402652010584,-6.19390911861184,-4.11024197137578;-7.46678251410560,8.56822989981309,11.2043255855097,15.2654942328664,33.4738990907830,47.9830070353326,34.7040045650604,26.2413120770515,20.1763824490189,15.5338843206769,11.7326411948090,12.1758072414718,12.5134699918411,8.40626539563750,7.30680994665188,9.06301574584210,9.06718553903426,3.28096035176137,2.95729086923455,7.59500845654168,8.78691717539524,2.64757399319922,-3.44893480787046,-4.90245885811243,-2.43438356403605;-4.25370721254760,4.62566966250763,4.01332627819630,6.79294390493924,19.1783116705422,34.7040045650604,38.8837181008703,30.5855303653521,24.4293160531473,18.9016685602971,14.2242047566833,13.2023412009645,11.9227142773337,7.14553870946421,...
    4.96732552922627,6.13321436882713,7.22759640328091,2.16334833830781,1.65935406464459,6.43593358182628,8.88682735180719,5.79370630741602,1.12312165810067,-0.760152330283094,1.12185084519372;-4.66530692581079,2.84351679390897,1.75273592864460,2.67066518158898,12.2583479560372,26.2413120770515,30.5855303653521,32.9565348690616,26.7182963922774,20.7877518714030,15.9490607233597,13.4096470381090,9.66184826535858,4.17684093190548,2.51542925992028,3.82203736299656,5.27124999576892,0.937242950967530,0.506456787370924,5.78263560277997,9.34050185558233,7.97815356483984,4.34695875838788,2.45629702312837,3.81219172463527;-2.51727840325899,1.09976231738547,0.308034614343833,0.511959074292315,8.40388068523053,20.1763824490189,24.4293160531473,26.7182963922774,30.3731349198695,25.5535568049251,20.4396793157684,16.9065249160539,10.9790177587369,3.83355066985249,1.75378340648248,1.92694003311363,3.09720245496635,0.678390445518196,0.370155199370253,4.27662157160065,7.42696415246535,7.74165558357469,5.97958182700411,4.59718762086779,4.99059712894902;-0.0222693428525085,-0.430958411928135,-0.748387958340454,0.145692285708609,6.38586268081686,15.5338843206769,18.9016685602971,20.7877518714030,25.5535568049251,31.2476011036791,29.2280699026235,24.8369253382793,15.6407092184352,5.11393100897667,1.86192844374648,-0.259923622107660,0.108944847748196,0.667967549294245,0.706832324773131,2.43720248780174,4.78076715908165,5.94757137106767,...
    5.79801014414371,5.30364220273057,4.61632444602688;7.77271764064578,-1.66921241769178,-2.12193478804064,0.717149484132317,5.87521395197408,11.7326411948090,14.2242047566833,15.9490607233597,20.4396793157684,29.2280699026235,42.3898726096171,43.1342264249106,28.6006605380589,8.97335954417316,1.99754411672603,-3.86497193326403,-5.46009164196636,0.0465581825178522,0.530018964555680,-2.42643238124481,-1.57744989779292,1.68779751335937,4.98136998663012,5.45107019210210,2.79964334374144;15.8892570422519,-0.921167853845433,-1.20656809658165,3.72055225536004,8.76329751567099,12.1758072414718,13.2023412009645,13.4096470381090,16.9065249160539,24.8369253382793,43.1342264249106,63.3842078331455,53.6739782424853,20.9959605250820,5.23770283200580,-5.49557630039091,-9.45682433094533,0.382393899491493,0.512709493987382,-8.44118453865293,-11.1184338227843,-6.65300768370212,-0.0872026152592587,1.75553693441792,-1.80261495909636;20.2557909574935,2.13736114488578,3.80442620051941,9.80684013028351,12.4345759540831,12.5134699918411,11.9227142773337,9.66184826535858,10.9790177587369,15.6407092184352,28.6006605380589,53.6739782424853,74.0923284109850,50.4166914996434,19.8452426569222,3.71951631884604,-3.01578681013651,8.15078339334762,6.69901056709512,-9.45165232762387,-17.8340454906504,-14.3408557431692,-6.37686799577174,-3.23741959571740,-5.76083660378918;15.0499455736291,5.79125843861373,10.3758286758611,14.8095004677359,11.9812162569625,...
    8.40626539563750,7.14553870946421,4.17684093190548,3.83355066985249,5.11393100897667,8.97335954417316,20.9959605250820,50.4166914996434,67.5792555276168,41.7285131724212,19.3591204209366,13.3170246203033,18.5797792828178,16.7327844668484,-0.0536558968531892,-12.3152971584790,-12.6451231579551,-6.81506327449899,-3.79578089796728,-3.52802480741186;6.71439781940967,6.89947737024852,12.2871479790389,14.9861322117183,12.0062466666891,7.30680994665188,4.96732552922627,2.51542925992028,1.75378340648248,1.86192844374648,1.99754411672603,5.23770283200580,19.8452426569222,41.7285131724212,49.6344789006755,33.0348328931340,22.6584694458312,24.5634169924077,20.7336566999343,8.98471096946229,-1.75122569976986,-5.08698626831735,-2.93401154984920,-0.424714044749733,0.977062771119680;-0.215644526778094,6.20779611628890,11.0248476286196,12.0969247937467,11.7524734022862,9.06301574584210,6.13321436882713,3.82203736299656,1.92694003311363,-0.259923622107660,-3.86497193326403,-5.49557630039091,3.71951631884604,19.3591204209366,33.0348328931340,46.8569681103014,37.1227032306882,28.4164102999117,25.4293672761571,18.3220080574232,10.7806882026212,4.57150248986407,3.24521768565963,5.22881351643142,7.14447922665191;-7.53674644535830,5.01156071334007,10.1626689024442,9.97444495277138,9.75492382009717,9.06718553903426,7.22759640328091,5.27124999576892,3.09720245496635,0.108944847748196,-5.46009164196636,-9.45682433094533,-3.01578681013651,...
    13.3170246203033,22.6584694458312,37.1227032306882,52.9771827487890,39.2546315259853,25.6706839511040,21.0642016455489,14.3464782482841,7.91032794663135,4.32650127986558,5.90700884911719,9.51235482158181;6.37908866685120,5.23163176987512,11.8755984854200,12.1336595171325,7.72249685023660,3.28096035176137,2.16334833830781,0.937242950967530,0.678390445518196,0.667967549294245,0.0465581825178522,0.382393899491493,8.15078339334762,18.5797792828178,24.5634169924077,28.4164102999117,39.2546315259853,57.5148887494567,41.1198492066879,18.8077874168484,3.85710787915015,-1.17371343032634,2.16164491967028,5.76647136103349,6.43257195016135;7.58491557425687,5.21605272525785,9.99589074642841,10.7689256498738,7.37892285808970,2.95729086923455,1.65935406464459,0.506456787370924,0.370155199370253,0.706832324773131,0.530018964555680,0.512709493987382,6.69901056709512,16.7327844668484,20.7336566999343,25.4293672761571,25.6706839511040,41.1198492066879,53.8799571804681,35.0471044567753,12.1141848108371,1.36372447126756,2.46782018459634,6.42354249537114,6.99112536566596;-12.4662256646260,1.85978240005883,3.45373914111269,2.81499883719888,5.70061848255443,7.59500845654168,6.43593358182628,5.78263560277997,4.27662157160065,2.43720248780174,-2.42643238124481,-8.44118453865293,-9.45165232762387,-0.0536558968531892,8.98471096946229,18.3220080574232,21.0642016455489,18.8077874168484,35.0471044567753,60.8092232209226,46.7665270102987,22.3072793313734,...
    8.53298603025637,8.43007400326799,12.5220181692328;-20.3334206261375,-1.98976643262044,-3.60052534762597,-5.95682083402728,0.995525690391575,8.78691717539524,8.88682735180719,9.34050185558233,7.42696415246535,4.78076715908165,-1.57744989779292,-11.1184338227843,-17.8340454906504,-12.3152971584790,-1.75122569976986,10.7806882026212,14.3464782482841,3.85710787915015,12.1141848108371,46.7665270102987,78.0506003327905,54.3905828569881,24.2878979305115,16.4162605003120,21.0204849128214;-11.0202990043130,-1.83352733616632,-4.17205663582501,-7.54682005594304,-4.40395983807157,2.64757399319922,5.79370630741602,7.97815356483984,7.74165558357469,5.94757137106767,1.68779751335937,-6.65300768370212,-14.3408557431692,-12.6451231579551,-5.08698626831735,4.57150248986407,7.91032794663135,-1.17371343032634,1.36372447126756,22.3072793313734,54.3905828569881,79.0516969252596,52.3230640903561,30.1361212164833,28.2413484569709;5.31395653148383,0.200381018506610,-2.00656654985110,-5.30507835073042,-6.71402652010584,-3.44893480787046,1.12312165810067,4.34695875838788,5.97958182700411,5.79801014414371,4.98136998663012,-0.0872026152592587,-6.37686799577174,-6.81506327449899,-2.93401154984920,3.24521768565963,4.32650127986558,2.16164491967028,2.46782018459634,8.53298603025637,24.2878979305115,52.3230640903561,72.6527804669958,50.7963162745079,35.7490744589117;9.07737891954220,1.78445605294837,0.190204427590809,-3.39487727855270,-6.19390911861184,...
    -4.90245885811243,-0.760152330283094,2.45629702312837,4.59718762086779,5.30364220273057,5.45107019210210,1.75553693441792,-3.23741959571740,-3.79578089796728,-0.424714044749733,5.22881351643142,5.90700884911719,5.76647136103349,6.42354249537114,8.43007400326799,16.4162605003120,30.1361212164833,50.7963162745079,64.6248112328330,48.4861932835982;2.82651770353372,2.64615485644659,1.59433432378998,-2.12888973859252,-4.11024197137578,-2.43438356403605,1.12185084519372,3.81219172463527,4.99059712894902,4.61632444602688,2.79964334374144,-1.80261495909636,-5.76083660378918,-3.52802480741186,0.977062771119680,7.14447922665191,9.51235482158181,6.43257195016135,6.99112536566596,12.5220181692328,21.0204849128214,28.2413484569709,35.7490744589117,48.4861932835982,59.7366908390514];
covStates(:,:,4)=[65.1113537847464,57.6212179463206,46.2865195345761,41.3985136155485,37.1619087322619,31.7167866023194,27.8472129944912,24.9608941438934,22.3721068017361,20.2122867630819,19.5218688429700,19.8581592733187,18.9010795314532,17.3107961108061,16.7189752483202,16.2627191805213,13.0730772716576,11.0718819250364,9.90062115480064,9.28127706159290,8.27165561976471,6.43826931259395,4.43499403004708,2.60717638234063,1.87490990043223;57.6212179463206,68.7047078817304,57.4970188969540,47.5035576068998,41.6799354315705,34.7106282421940,29.8353500401384,26.4258764879887,23.3222750816593,20.5739384165772,19.6400509325692,20.1705400149328,19.6880793425978,18.2680461495366,18.0001950790376,18.0434767135578,14.5183634772315,12.3677824275506,11.1014987246596,10.2325144342892,8.89739907709928,6.73883111394463,4.33248232837791,2.29623364348226,1.35067748182447;46.2865195345761,57.4970188969540,60.0847897969091,50.2213012843078,41.8094653542813,35.1884065077802,30.4888843441675,27.1832487290321,24.1114748878850,21.4657740329523,20.4858796640510,20.9744554845210,20.7633096346180,19.4173620114267,19.2709141143814,19.2527448767479,16.0229699706731,14.0491310436395,12.6385989253233,11.6446307794350,9.96248493260986,7.62846570739715,5.24650627596249,3.28554690895135,2.33676852949923;41.3985136155485,47.5035576068998,50.2213012843078,54.1337976181797,46.1979654232229,37.4941403757244,32.8480620167725,29.6695586263516,...
    26.5135986653706,24.0862574219597,23.2974756118745,23.7955021948309,23.4331250186943,21.5774778978497,21.4675579356391,21.1986740961742,17.3102733676493,15.4890670802103,13.8887597129763,12.4161503971810,10.4481838485923,7.63576849002264,5.13451537677302,3.22722917305760,2.34986532516011;37.1619087322619,41.6799354315705,41.8094653542813,46.1979654232229,51.5609924749057,43.2985504059220,36.2206724149515,33.0894591075687,30.0742409948422,27.5379878512978,26.5005627428872,26.6644777609558,25.9828620849597,23.6466929495920,22.8419516880056,22.0937626180991,17.6542610583372,15.2606895590935,13.4648769495558,11.8082460269857,9.98879484131242,7.36163825044701,4.93355395091168,3.14445531612878,2.36291438219950;31.7167866023194,34.7106282421940,35.1884065077802,37.4941403757244,43.2985504059220,47.2717013969818,40.0740794116267,35.1601432805978,32.6256195856808,29.9076899344333,28.1375557509425,27.5321641338248,26.5485834532367,24.2424268675134,22.5085422995731,21.0942211632242,16.8713382664636,13.7125500584522,...
    11.8830917485755,10.6820112313720,9.29034728417671,7.48495804063082,5.57673431798753,4.06046756246026,3.48444277156466;27.8472129944912,29.8353500401384,30.4888843441675,32.8480620167725,36.2206724149515,40.0740794116267,43.6490798160482,37.8501423704850,33.5523124754331,30.9382795001591,28.8376953629112,27.7233891926362,26.2487783048825,23.9630605948905,21.9585310603541,20.0786693128178,16.0414257803555,12.8967379787739,11.0975939036618,10.1297015228385,9.05872834023055,7.75184303004415,6.19081111316702,4.92350672869426,4.63035811504703;24.9608941438934,26.4258764879887,27.1832487290321,29.6695586263516,33.0894591075687,35.1601432805978,37.8501423704850,41.8626067173072,36.4888288968266,32.0326372061761,29.9227664154704,28.3939266419876,26.3370827810484,23.7057543744551,21.5086434003543,19.3736290986700,15.4429140090662,12.4283842985899,10.8370374772401,9.97995360768762,9.08858368660505,8.01401521362707,6.66134688811938,5.60402588287641,5.44124906827518;22.3721068017361,23.3222750816593,24.1114748878850,26.5135986653706,30.0742409948422,32.6256195856808,33.5523124754331,36.4888288968266,40.6516794619737,35.1537112525614,30.9300450971289,28.5540182854975,26.2946344484776,23.7911379511868,20.9035687789669,18.4891708781401,14.9644833252678,11.4954210389700,9.84595877818955,9.16648501581297,8.53530397930319,8.05507022002741,7.27639846043703,6.59841169826374,6.57104672369896;20.2122867630819,20.5739384165772,...
    21.4657740329523,24.0862574219597,27.5379878512978,29.9076899344333,30.9382795001591,32.0326372061761,35.1537112525614,40.2938942239913,35.6258420784876,30.6427056684458,27.3626510122550,24.9254343273733,21.7303931456209,18.4488221628916,15.1938412871818,11.6427919549918,9.03357330926534,7.61975503074621,6.69622018123396,6.24045249159496,6.01294495537824,5.74018767538138,5.85488703055044;19.5218688429700,19.6400509325692,20.4858796640510,23.2974756118745,26.5005627428872,28.1375557509425,28.8376953629112,29.9227664154704,30.9300450971289,35.6258420784876,43.1899850979340,38.2784708655997,31.3064769105055,27.5452866415497,25.0240568230500,20.9841991371905,17.3707458992894,14.6104883241199,11.1419835600831,8.27504662315305,6.31588621221399,4.68580997376940,4.00932866857303,3.66980012030712,3.63103764909243;19.8581592733187,20.1705400149328,20.9744554845210,23.7955021948309,26.6644777609558,27.5321641338248,27.7233891926362,28.3939266419876,28.5540182854975,30.6427056684458,38.2784708655997,48.5380048645016,...
    42.0216542507911,33.2364395076378,32.1458803915595,28.3508942527038,23.0832557322845,21.3601370602944,17.2555595543051,12.8114841460071,9.73012311081682,6.61066339860173,5.08216349419049,4.21258267787987,3.57700519882438;18.9010795314532,19.6880793425978,20.7633096346180,23.4331250186943,25.9828620849597,26.5485834532367,26.2487783048825,26.3370827810484,26.2946344484776,27.3626510122550,31.3064769105055,...
    42.0216542507911,52.7825739250406,44.7733389035753,41.0691483357663,38.7275404134195,31.7378246304664,28.2575201956681,22.4860251279806,16.2866182100269,12.2794417607475,9.32067335258732,8.28514253804218,7.48137135247518,5.97024288601077;17.3107961108061,18.2680461495366,19.4173620114267,21.5774778978497,23.6466929495920,24.2424268675134,23.9630605948905,23.7057543744551,23.7911379511868,24.9254343273733,27.5452866415497,33.2364395076378,44.7733389035753,53.9951464947342,49.8036040859568,45.0222011107760,39.3112016259021,31.6509660428218,22.5625803632591,15.2685185744029,10.3607079819248,8.51357735296230,8.70368379766978,8.38199340194058,6.27807976864847;16.7189752483202,18.0001950790376,19.2709141143814,21.4675579356391,22.8419516880056,22.5085422995731,21.9585310603541,21.5086434003543,20.9035687789669,21.7303931456209,25.0240568230500,32.1458803915595,41.0691483357663,49.8036040859568,63.1061135925138,58.7414471370163,50.8436421523001,44.8163218294713,31.6016949470013,20.3764757498427,12.6925406995745,8.78561301881692,8.31204359324081,7.82473862552048,5.17593013655696;16.2627191805213,18.0434767135578,19.2527448767479,21.1986740961742,22.0937626180991,21.0942211632242,20.0786693128178,19.3736290986700,18.4891708781401,18.4488221628916,20.9841991371905,28.3508942527038,38.7275404134195,45.0222011107760,58.7414471370163,70.4437460450180,61.8122813200347,55.8612108309575,44.4181551249925,30.6064653872930,...
    20.6220570791060,14.3658257693051,12.3926120357220,11.4759004344674,8.50092008894901;13.0730772716576,14.5183634772315,16.0229699706731,17.3102733676493,17.6542610583372,16.8713382664636,16.0414257803555,15.4429140090662,14.9644833252678,15.1938412871818,17.3707458992894,23.0832557322845,31.7378246304664,39.3112016259021,50.8436421523001,61.8122813200347,69.7235188537475,63.9793085590785,51.7821087846089,38.4778451178486,26.8688274472963,20.5147668450644,18.5534930876352,17.6376115794853,14.6388584128503;11.0718819250364,12.3677824275506,14.0491310436395,15.4890670802103,15.2606895590935,13.7125500584522,12.8967379787739,12.4283842985899,11.4954210389700,11.6427919549918,14.6104883241199,21.3601370602944,28.2575201956681,31.6509660428218,44.8163218294713,55.8612108309575,63.9793085590785,77.3211195084415,68.1575579525177,53.1659307495083,39.4567646881240,29.3742034827177,25.7345144111849,24.5027556710962,22.0847731326531;9.90062115480064,11.1014987246596,12.6385989253233,13.8887597129763,13.4648769495558,...
    11.8830917485755,11.0975939036618,10.8370374772401,9.84595877818955,9.03357330926534,11.1419835600831,17.2555595543051,22.4860251279806,22.5625803632591,31.6016949470013,44.4181551249925,51.7821087846089,68.1575579525177,79.2338709540036,69.0940380467572,55.7055389690711,43.6201463794592,36.9778373025157,34.4867802288549,32.7814277410591;9.28127706159290,10.2325144342892,11.6446307794350,12.4161503971810,11.8082460269857,10.6820112313720,10.1297015228385,9.97995360768762,9.16648501581297,7.61975503074621,8.27504662315305,12.8114841460071,16.2866182100269,15.2685185744029,20.3764757498427,30.6064653872930,38.4778451178486,53.1659307495083,69.0940380467572,77.2793399428354,68.8025358477076,57.0045061379912,49.1922904983969,45.0825398162078,43.5018994290493;8.27165561976471,8.89739907709928,9.96248493260986,10.4481838485923,9.98879484131242,9.29034728417671,9.05872834023055,9.08858368660505,8.53530397930319,6.69622018123396,6.31588621221399,9.73012311081682,12.2794417607475,10.3607079819248,12.6925406995745,20.6220570791060,26.8688274472963,39.4567646881240,55.7055389690711,68.8025358477076,76.0483558649711,68.3778462675754,60.3048059360899,55.1668785858327,53.3510076795231;6.43826931259395,6.73883111394463,7.62846570739715,7.63576849002264,7.36163825044701,7.48495804063082,7.75184303004415,8.01401521362707,8.05507022002741,6.24045249159496,4.68580997376940,6.61066339860173,9.32067335258732,8.51357735296230,...
    8.78561301881692,14.3658257693051,20.5147668450644,29.3742034827177,43.6201463794592,57.0045061379912,68.3778462675754,74.9290873571941,70.5860716093331,64.8020551050686,62.3889868745278;4.43499403004708,4.33248232837791,5.24650627596249,5.13451537677302,4.93355395091168,5.57673431798753,6.19081111316702,6.66134688811938,7.27639846043703,6.01294495537824,4.00932866857303,5.08216349419049,8.28514253804218,8.70368379766978,8.31204359324081,12.3926120357220,18.5534930876352,25.7345144111849,36.9778373025157,49.1922904983969,60.3048059360899,70.5860716093331,77.9539929326179,74.3614563466212,70.9244966764649;2.60717638234063,2.29623364348226,3.28554690895135,3.22722917305760,3.14445531612878,4.06046756246026,4.92350672869426,5.60402588287641,6.59841169826374,5.74018767538138,3.66980012030712,4.21258267787987,7.48137135247518,8.38199340194058,7.82473862552048,11.4759004344674,17.6376115794853,24.5027556710962,34.4867802288549,45.0825398162078,55.1668785858327,64.8020551050686,74.3614563466212,80.3455859157465,...
    77.8094123367003;1.87490990043223,1.35067748182447,2.33676852949923,2.34986532516011,2.36291438219950,3.48444277156466,4.63035811504703,5.44124906827518,6.57104672369896,5.85488703055044,3.63103764909243,3.57700519882438,5.97024288601077,6.27807976864847,5.17593013655696,8.50092008894901,14.6388584128503,22.0847731326531,32.7814277410591,43.5018994290493,53.3510076795231,62.3889868745278,70.9244966764649,77.8094123367003,83.9868902784835];
covStates(:,:,5)=[55.6695138135351,33.8602729293450,27.1604901698488,28.0152595210164,19.0874183878906,8.48815304500790,7.56183441740966,6.73134278744991,5.43314690574577,2.20889649032588,0.466226453781336,-0.171524495080273,-0.468853862440327,4.17498150188241,7.48282401520442,2.93821274913768,-2.16405509854264,1.71634533303462,1.56501126366676,-5.22410379721469,-8.44640787105269,-4.68552394235714,1.22251919908922,2.72252579658210,-0.473268676528509;33.8602729293450,55.1857256225024,53.9498900357743,46.6761185916503,37.2039547998096,19.5776530608383,11.6582401574036,8.54163724483447,6.26642788706633,1.12509708960587,-6.28213222433507,-10.2920468778544,-8.82965390820725,-2.59933826901477,3.08136095633613,2.06149179244731,-2.57899837029304,-6.46627337703099,-6.14593281063864,-5.26887673876649,-7.22412807391255,-7.44801035923241,-5.67598866065765,-4.17402293172972,-3.24315414601088;27.1604901698488,53.9498900357743,67.3671420998487,59.2810653961734,45.4365485831731,25.5952005525636,15.5831644501225,11.7840037656820,8.56031487500004,1.92949291009610,-7.48269298052371,-12.8935425803171,-11.0409947599559,-2.46398261507361,4.40392585954482,2.58459416875158,-2.54185849884548,-6.90191637841730,-7.35773535427313,-7.36475530855097,-10.4555225106331,-10.3750480539769,-8.43388509089370,-6.76979041764888,-5.71464262798761;28.0152595210164,46.6761185916503,59.2810653961734,71.3711868151990,60.1513618151353,38.2653561666905,...
    27.1803488642939,22.3424556513133,15.7100761747281,5.85712207743924,-5.26316667185055,-11.8479392070324,-11.0794468540500,-5.02032909496823,0.220585581618569,-0.512024016241786,-2.15018487247995,-4.37520959582283,-6.31351571359934,-9.08074247379992,-13.7575751943330,-14.2520135994889,-12.3752410421859,-10.3488457532746,-8.71337312824188;19.0874183878906,37.2039547998096,45.4365485831731,60.1513618151353,71.5387247586737,54.6349334048355,41.8858410280743,34.6233440365918,25.7808902477245,14.1451556160721,2.11814949292405,-5.87703090428430,-7.96607522567497,-7.71817472962086,-5.70099507095447,-4.15152363718404,-1.36469408497228,-2.15485540222869,-5.14178692399225,-7.78107869290461,-12.1033499828654,-14.4197752883500,-14.1817882556796,-12.2432139774715,-9.97084533783263;8.48815304500790,19.5776530608383,25.5952005525636,38.2653561666905,54.6349334048355,63.9593238625023,55.2533678305012,45.9888739371672,35.2692980829173,22.9488299976633,11.1943418498971,2.43441502591364,-2.59895912791485,-7.24674281127403,...
    -9.07187908794689,-7.20169713741346,-1.25268760799844,-0.378739575023543,-3.65441592907677,-6.64096067231484,-8.66259015429937,-10.3615805327534,-11.0905393414534,-10.0185308315703,-8.28090266584505;7.56183441740966,11.6582401574036,15.5831644501225,27.1803488642939,41.8858410280743,55.2533678305012,64.6984792890374,57.0031787861904,41.6788694205755,26.0486679131049,12.7787489172974,2.86944687460067,-3.57632726887160,-8.60336170511126,-10.8550787392070,-9.40907629726586,-2.03137266458439,0.764532629454872,-2.64366275091502,-6.57947520011811,-7.97644031879419,-8.57371869366686,-8.23035770152738,-7.25752602116806,-6.36326400147914;6.73134278744991,8.54163724483447,11.7840037656820,22.3424556513133,34.6233440365918,45.9888739371672,57.0031787861904,65.3328039204267,52.2067421798040,31.9654107275568,15.1636158728025,3.08552717861696,-4.86716966129234,-10.0393256285007,-12.1148682882660,-10.7406849579277,-3.29269127682367,0.370209909293462,-2.45737921725850,-6.14984197431451,-6.75894125029951,-6.44354359372853,-5.75347215805217,-4.93426482290209,-4.43183811090625;5.43314690574577,6.26642788706633,8.56031487500004,15.7100761747281,25.7808902477245,35.2692980829173,41.6788694205755,52.2067421798040,61.5873340871597,47.7624085467597,26.3576139259150,10.7935166699271,0.831280675238660,-5.58691626034991,-9.36099750087333,-9.68854403262894,-4.08306684591997,-1.37677955823272,-2.97886417156862,-4.92718824895066,...
    -4.21335451738660,-3.50334507463596,-2.82603107507817,-2.17175533559833,-1.75484423328806;2.20889649032588,1.12509708960587,1.92949291009610,5.85712207743924,14.1451556160721,22.9488299976633,26.0486679131049,31.9654107275568,47.7624085467597,62.9462172633248,49.8543604619894,29.7848214704037,15.7954798775897,4.72244468885664,-4.00870546601633,-7.95989088887742,-3.85810178563123,-0.987334523022716,-1.88305352244360,-2.55501741931131,-1.12028538692006,-0.377394223165656,0.489039607354747,0.801493925236912,1.41493267666535;0.466226453781336,-6.28213222433507,-7.48269298052371,-5.26316667185055,2.11814949292405,11.1943418498971,12.7787489172974,15.1636158728025,26.3576139259150,49.8543604619894,69.9032118379336,58.7660565033360,38.1857200841916,19.1812247351940,5.06673336104039,-1.86950981385977,-0.623331880614520,2.45997380551192,2.44580833883566,0.935228166226296,2.12988198345015,3.21470898559714,4.07345276678935,4.29433259881150,4.07075150254910;-0.171524495080273,-10.2920468778544,-12.8935425803171,...
    -11.8479392070324,-5.87703090428430,2.43441502591364,2.86944687460067,3.08552717861696,10.7935166699271,29.7848214704037,58.7660565033360,77.6593196813021,63.1269686912835,35.6419212978012,15.8128838204499,7.36890042136141,5.80761991043094,7.42625006161624,8.01257641951492,5.85089608806817,5.96917663164842,6.75548129401725,6.69575782886329,6.50652375879168,6.14096905899489;-0.468853862440327,-8.82965390820725,...
    -11.0409947599559,-11.0794468540500,-7.96607522567497,-2.59895912791485,-3.57632726887160,-4.86716966129234,0.831280675238660,15.7954798775897,38.1857200841916,63.1269686912835,74.8901632591667,54.3898391635645,28.1344365802940,16.7932222723415,14.3218336114534,14.2887391273455,13.0209646538286,10.4567761980765,9.07016813094358,8.88056700059441,7.89742069967112,7.46716770069211,7.53216036789469;4.17498150188241,-2.59933826901477,-2.46398261507361,-5.02032909496823,-7.71817472962086,-7.24674281127403,-8.60336170511126,-10.0393256285007,-5.58691626034991,4.72244468885664,19.1812247351940,35.6419212978012,54.3898391635645,68.2093542850770,51.2589997968140,30.6716067183146,21.4662843019386,20.6759109439758,18.8929946512005,14.4839580408173,11.1024701342924,10.0747858457603,10.0835845565993,10.0099798852742,9.27981245033238;7.48282401520442,3.08136095633613,4.40392585954482,0.220585581618569,-5.70099507095447,-9.07187908794689,-10.8550787392070,-12.1148682882660,-9.36099750087333,-4.00870546601633,5.06673336104039,15.8128838204499,28.1344365802940,51.2589997968140,69.6552503752206,53.9635475899766,30.0138153578779,25.7870089309956,25.4404557063181,21.0437159502376,16.7290668419198,14.4100669733868,13.9566862291528,14.0896301781058,12.4462977008965;2.93821274913768,2.06149179244731,2.58459416875158,-0.512024016241786,-4.15152363718404,-7.20169713741346,-9.40907629726586,-10.7406849579277,-9.68854403262894,...
    -7.95989088887742,-1.86950981385977,7.36890042136141,16.7932222723415,30.6716067183146,53.9635475899766,70.3620112481106,48.6337355790127,31.9211781842193,31.4191069701555,29.0700388604427,24.9265923461637,20.5983019133413,17.2128481725112,16.8391187929583,16.0523978360380;-2.16405509854264,-2.57899837029304,-2.54185849884548,-2.15018487247995,-1.36469408497228,-1.25268760799844,-2.03137266458439,-3.29269127682367,-4.08306684591997,-3.85810178563123,-0.623331880614520,5.80761991043094,14.3218336114534,21.4662843019386,30.0138153578779,48.6337355790127,63.7026150518575,46.6961728256057,33.5295205246110,30.5147183862566,26.1750958543505,21.6056704504742,16.5489497896529,15.5140396161771,16.4068154624278;1.71634533303462,-6.46627337703099,-6.90191637841730,-4.37520959582283,-2.15485540222869,-0.378739575023543,0.764532629454872,0.370209909293462,-1.37677955823272,-0.987334523022716,2.45997380551192,7.42625006161624,14.2887391273455,20.6759109439758,25.7870089309956,31.9211781842193,46.6961728256057,65.6622991779044,...
    49.4660076528715,32.4440472429410,23.6312494004877,19.2442025596338,16.3721787130175,15.4820432475627,14.9870940531895;1.56501126366676,-6.14593281063864,-7.35773535427313,-6.31351571359934,-5.14178692399225,-3.65441592907677,-2.64366275091502,-2.45737921725850,-2.97886417156862,-1.88305352244360,2.44580833883566,8.01257641951492,13.0209646538286,18.8929946512005,25.4404557063181,31.4191069701555,33.5295205246110,49.4660076528715,65.1736269858933,48.0663590034473,29.4983872122291,21.4943244020658,17.7002253641323,17.2758533666646,16.2037550245846;-5.22410379721469,-5.26887673876649,-7.36475530855097,-9.08074247379992,-7.78107869290461,-6.64096067231484,-6.57947520011811,-6.14984197431451,-4.92718824895066,-2.55501741931131,0.935228166226296,5.85089608806817,10.4567761980765,14.4839580408173,21.0437159502376,29.0700388604427,30.5147183862566,32.4440472429410,48.0663590034473,63.5562685736950,48.4162512857207,30.4541652973896,21.2148588081338,19.5458390887160,19.9397322658007;-8.44640787105269,-7.22412807391255,-10.4555225106331,-13.7575751943330,-12.1033499828654,-8.66259015429937,-7.97644031879419,-6.75894125029951,-4.21335451738660,-1.12028538692006,2.12988198345015,5.96917663164842,9.07016813094358,11.1024701342924,16.7290668419198,24.9265923461637,26.1750958543505,23.6312494004877,29.4983872122291,48.4162512857207,65.0226568385688,48.0710492296142,28.8157671576827,23.0867528600051,23.5795518074400;...
    -4.68552394235714,-7.44801035923241,-10.3750480539769,-14.2520135994889,-14.4197752883500,-10.3615805327534,-8.57371869366686,-6.44354359372853,-3.50334507463596,-0.377394223165656,3.21470898559714,6.75548129401725,8.88056700059441,10.0747858457603,14.4100669733868,20.5983019133413,21.6056704504742,19.2442025596338,21.4943244020658,30.4541652973896,48.0710492296142,61.3178625869795,43.8596617812672,29.4208612514201,26.3934365995185;1.22251919908922,-5.67598866065765,-8.43388509089370,-12.3752410421859,-14.1817882556796,-11.0905393414534,-8.23035770152738,-5.75347215805217,-2.82603107507817,0.489039607354747,4.07345276678935,6.69575782886329,7.89742069967112,10.0835845565993,13.9566862291528,17.2128481725112,16.5489497896529,16.3721787130175,17.7002253641323,21.2148588081338,28.8157671576827,43.8596617812672,53.7383194107891,40.0548566932287,29.2545385613087;2.72252579658210,-4.17402293172972,-6.76979041764888,-10.3488457532746,-12.2432139774715,-10.0185308315703,-7.25752602116806,-4.93426482290209,-2.17175533559833,...
    0.801493925236912,4.29433259881150,6.50652375879168,7.46716770069211,10.0099798852742,14.0896301781058,16.8391187929583,15.5140396161771,15.4820432475627,17.2758533666646,19.5458390887160,23.0867528600051,29.4208612514201,40.0548566932287,48.6383291069905,37.5675861731830;-0.473268676528509,-3.24315414601088,-5.71464262798761,-8.71337312824188,-9.97084533783263,-8.28090266584505,-6.36326400147914,-4.43183811090625,-1.75484423328806,1.41493267666535,4.07075150254910,6.14096905899489,7.53216036789469,9.27981245033238,12.4462977008965,16.0523978360380,16.4068154624278,14.9870940531895,16.2037550245846,19.9397322658007,23.5795518074400,26.3934365995185,29.2545385613087,37.5675861731830,45.5634253642452];
covStates(:,:,6)=[92.4773705748397,23.9132917419201,9.42108337896781,18.6033229615450,12.7804628017197,10.5357788467363,-5.64771958947045,-16.9658664389210,-14.7288980443676,-5.55458166577810,4.78286109500975,6.95110229329951,0.00879075987156321,-4.56967576746686,4.81515552944994,2.62036551470727,-7.29029260769826,7.60994352313745,2.47911656495987,-19.0672555520342,-20.5734048413298,-13.2075273562642,5.91579472308194,12.5261436233794,0.396845671600897;23.9132917419201,19.8450315090894,12.1708804687979,11.7886010742092,11.5179976243568,4.62518859680709,-0.911063824710008,-3.39571454358632,-3.62056110128200,-3.37797266880504,-1.60834775075760,0.795631607365374,2.08528253489662,1.60079192483396,1.51828664833571,1.54391683150779,0.525534472247695,1.55170824084267,1.33105278816537,-0.281924859658859,-0.573822155925386,1.46664945053241,5.10400561369775,7.19978550743818,6.73941584361868;9.42108337896781,12.1708804687979,18.4617440631252,13.6106632397527,13.6162692554101,6.02190283260239,-1.90865324567817,-4.15829132935408,-3.60630137677266,-3.54582614084532,-2.26821098324758,0.517292195308823,3.14509536372528,3.80829791379001,3.32607430412317,2.91471516655482,3.34912935358682,5.20923042016220,4.29738386466969,2.73025296949213,1.58166856773255,3.14843379754112,5.84609591051610,7.77238354115449,8.43717665704851;18.6033229615450,11.7886010742092,13.6106632397527,22.8077245543831,18.0118684303035,8.05278768561534,...
    -6.38161937025231,-10.3356440281175,-9.22371125078008,-7.43863418366843,-4.56101523134668,-0.216773407540774,3.89543459828660,4.90389717333831,4.66127606211765,3.47030457098709,2.74033891407396,5.72211439376269,5.28391368902764,2.25114917243912,-0.628514051806710,0.228684995216133,4.69411991158734,7.23959680984727,7.04380122502727;12.7804628017197,11.5179976243568,13.6162692554101,18.0118684303035,28.3756287378391,14.9822828140061,-2.70794976788686,-10.2510817187662,-8.53945157722774,-5.95040326389395,-3.21645466374053,0.141944034333193,3.38234829455764,4.89012956399443,5.55534520851414,3.63735667703317,3.18406810374213,7.04176943959671,6.21288060544659,2.52251449794559,-0.952991719117629,-1.84336163229673,2.16951897880099,5.07738907260155,4.81286211137161;10.5357788467363,4.62518859680709,6.02190283260239,8.05278768561534,14.9822828140061,25.1400759046123,14.9633265861569,6.00136334127404,4.76464855535492,7.51922776048866,7.13633984234143,3.24182864983849,-0.0603781670555945,0.545855562396198,4.27928625063530,...
    2.85871315997680,0.566656257357205,4.21827660401795,3.73962787068916,-0.753303064804990,-1.24908791135452,-1.60150240414188,0.662552139993113,1.28530643124126,-0.916697712307544;-5.64771958947045,-0.911063824710008,-1.90865324567817,-6.38161937025231,-2.70794976788686,14.9633265861569,35.1225245436802,30.9826586851182,27.6038229476860,26.4090813917774,19.8222466292767,7.15767944253354,-2.03667096553156,-2.64966773992657,0.0770085705865255,1.07303373835947,0.291056169731772,-1.76758780042540,-1.13793473179759,-0.106196239349976,3.60492394898904,4.86921724878281,1.46895532474066,-1.81855900912568,-3.24303790466327;-16.9658664389210,-3.39571454358632,-4.15829132935408,-10.3356440281175,-10.2510817187662,6.00136334127404,30.9826586851182,45.1972837925866,40.7682478688784,36.4894811171374,25.2700241602827,7.43626844690649,-4.35997810175267,-5.76338785540435,-5.10636014339218,-1.97130033648324,0.480151706326002,-4.79454189609896,-4.15180389292841,0.737869884021180,5.24783031692334,7.66766914332693,2.78565563257161,-1.86635253464110,-1.48161111587469;-14.7288980443676,-3.62056110128200,-3.60630137677266,-9.22371125078008,-8.53945157722774,4.76464855535492,27.6038229476860,40.7682478688784,49.2446688939519,45.6795508707621,29.5252744555428,5.16911980987520,-9.61007286765810,-10.8114296082780,-9.75868776317025,-6.02803080806971,-0.824738162216459,-3.90437152964992,-5.06206747182193,-2.73036755479272,...
    0.908686837256395,4.71834252437230,2.56658358594838,-1.04351516389379,-1.45339066300716;-5.55458166577810,-3.37797266880504,-3.54582614084532,-7.43863418366843,-5.95040326389395,7.51922776048866,26.4090813917774,36.4894811171374,45.6795508707621,55.9881212791662,43.6514426662486,9.93469796222488,-13.2832667690708,-15.2969484244819,-12.3359491619840,-10.3304662026549,-4.63756925753954,-2.46321798994156,-5.29194412619753,-8.92651482455280,-6.43200943191655,-1.75789464009488,-0.702979795889193,-2.39982181081864,-5.05836416758705;4.78286109500975,-1.60834775075760,-2.26821098324758,-4.56101523134668,-3.21645466374053,7.13633984234143,19.8222466292767,25.2700241602827,29.5252744555428,43.6514426662486,56.4879305696040,34.0017967352276,1.72485193969848,-8.59895358213287,-7.64272602475186,-9.67278314457806,-7.57048193635153,-1.76542823755722,-3.67359276000700,-11.5811131635602,-11.7605325347021,-7.56180044535635,-3.81790940876935,-3.11883524920612,-6.55269611082009;6.95110229329951,0.795631607365374,0.517292195308823,...
    -0.216773407540774,0.141944034333193,3.24182864983849,7.15767944253354,7.43626844690649,5.16911980987520,9.93469796222488,34.0017967352276,53.2006551201314,37.5152715859299,15.4412487845563,7.48457869608750,1.69725306456590,-1.89147831827456,1.32414050480706,2.23816143063194,-4.01425787953618,-7.55094808541236,-6.64715100035760,-3.30034026108823,-1.40659213221705,-2.07722914612490;0.00879075987156321,...
    2.08528253489662,3.14509536372528,3.89543459828660,3.38234829455764,-0.0603781670555945,-2.03667096553156,-4.35997810175267,-9.61007286765810,-13.2832667690708,1.72485193969848,37.5152715859299,59.7155797675980,44.7773950005295,23.6872126249230,15.0419986873052,10.2867275974593,7.44535689098688,8.92865721758249,8.15662452212477,2.92276952642247,-0.928795572640678,-0.214150561175240,1.35299628037947,4.39659721074060;-4.56967576746686,1.60079192483396,3.80829791379001,4.90389717333831,4.89012956399443,0.545855562396198,-2.64966773992657,-5.76338785540435,-10.8114296082780,-15.2969484244819,-8.59895358213287,15.4412487845563,44.7773950005295,56.5751628200309,37.8203840732640,20.3064153793782,17.7523505046127,13.7943682884086,10.9358068943459,11.9597833381183,8.15858880439622,2.20692296620118,0.925687564792643,1.80648630647397,5.51936228519465;4.81515552944994,1.51828664833571,3.32607430412317,4.66127606211765,5.55534520851414,4.27928625063530,0.0770085705865255,-5.10636014339218,-9.75868776317025,-12.3359491619840,-7.64272602475186,7.48457869608750,23.6872126249230,37.8203840732640,50.5567811262763,36.0267512290389,21.5229007099950,21.6212508611315,17.6388359925817,11.9335338722673,8.21479732534940,4.56458312200866,4.33009765711244,5.27098146930465,4.86850728556293;2.62036551470727,1.54391683150779,2.91471516655482,3.47030457098709,3.63735667703317,2.85871315997680,1.07303373835947,-1.97130033648324,...
    -6.02803080806971,-10.3304662026549,-9.67278314457806,1.69725306456590,15.0419986873052,20.3064153793782,36.0267512290389,52.6258535141836,37.6429273778615,26.0221565226345,25.2698743276596,20.1154473169106,13.2528440139144,9.04767643570268,8.59623530087994,9.90654514905407,9.81591972530337;-7.29029260769826,0.525534472247695,3.34912935358682,2.74033891407396,3.18406810374213,0.566656257357205,0.291056169731772,0.480151706326002,-0.824738162216459,-4.63756925753954,-7.57048193635153,-1.89147831827456,10.2867275974593,17.7523505046127,21.5229007099950,37.6429273778615,50.9566054526190,36.6093908509986,23.8228848504800,20.5158358369382,12.8996919192979,6.80957915648636,5.82313196344093,6.76698619720571,9.62737791259404;7.60994352313745,1.55170824084267,5.20923042016220,5.72211439376269,7.04176943959671,4.21827660401795,-1.76758780042540,-4.79454189609896,-3.90437152964992,-2.46321798994156,-1.76542823755722,1.32414050480706,7.44535689098688,13.7943682884086,21.6212508611315,26.0221565226345,36.6093908509986,...
    52.7467507553735,37.9169199894629,17.3357061745695,7.02185159858676,-0.331036876801312,1.94789829479953,5.02766136665644,4.41374687696034;2.47911656495987,1.33105278816537,4.29738386466969,5.28391368902764,6.21288060544659,3.73962787068916,-1.13793473179759,-4.15180389292841,-5.06206747182193,-5.29194412619753,-3.67359276000700,2.23816143063194,8.92865721758249,10.9358068943459,17.6388359925817,25.2698743276596,23.8228848504800,37.9169199894629,53.8401909646114,38.6507723240261,17.8182427670904,4.88430658725573,1.35573251610327,5.62258771132742,7.23177091210702;-19.0672555520342,-0.281924859658859,2.73025296949213,2.25114917243912,2.52251449794559,-0.753303064804990,-0.106196239349976,0.737869884021180,-2.73036755479272,-8.92651482455280,-11.5811131635602,-4.01425787953618,8.15662452212477,11.9597833381183,11.9335338722673,20.1154473169106,20.5158358369382,17.3357061745695,38.6507723240261,69.0407953227603,50.4721216646804,25.0197633538726,8.68426069638420,8.07850649374198,14.8126523282230;-20.5734048413298,...
    -0.573822155925386,1.58166856773255,-0.628514051806710,-0.952991719117629,-1.24908791135452,3.60492394898904,5.24783031692334,0.908686837256395,-6.43200943191655,-11.7605325347021,-7.55094808541236,2.92276952642247,8.15858880439622,8.21479732534940,13.2528440139144,12.8996919192979,7.02185159858676,17.8182427670904,50.4721216646804,75.9968226620634,53.3344079165803,21.0792722143554,10.1835442239764,...
    15.5032957956173;-13.2075273562642,1.46664945053241,3.14843379754112,0.228684995216133,-1.84336163229673,-1.60150240414188,4.86921724878281,7.66766914332693,4.71834252437230,-1.75789464009488,-7.56180044535635,-6.64715100035760,-0.928795572640678,2.20692296620118,4.56458312200866,9.04767643570268,6.80957915648636,-0.331036876801312,4.88430658725573,25.0197633538726,53.3344079165803,82.0329350072126,53.0033369734705,21.1505642732596,16.5655771407877;5.91579472308194,5.10400561369775,5.84609591051610,4.69411991158734,2.16951897880099,0.662552139993113,1.46895532474066,2.78565563257161,2.56658358594838,-0.702979795889193,-3.81790940876935,-3.30034026108823,-0.214150561175240,0.925687564792643,4.33009765711244,8.59623530087994,5.82313196344093,1.94789829479953,1.35573251610327,8.68426069638420,21.0792722143554,53.0033369734705,79.5401368685907,49.0744923891711,24.5660118766340;12.5261436233794,7.19978550743818,7.77238354115449,7.23959680984727,5.07738907260155,1.28530643124126,-1.81855900912568,-1.86635253464110,-1.04351516389379,-2.39982181081864,-3.11883524920612,-1.40659213221705,1.35299628037947,1.80648630647397,5.27098146930465,9.90654514905407,6.76698619720571,5.02766136665644,5.62258771132742,8.07850649374198,10.1835442239764,21.1505642732596,49.0744923891711,69.0043607615904,46.0670968831061;0.396845671600897,6.73941584361868,8.43717665704851,7.04380122502727,4.81286211137161,-0.916697712307544,...
    -3.24303790466327,-1.48161111587469,-1.45339066300716,-5.05836416758705,-6.55269611082009,-2.07722914612490,4.39659721074060,5.51936228519465,4.86850728556293,9.81591972530337,9.62737791259404,4.41374687696034,7.23177091210702,14.8126523282230,15.5032957956173,16.5655771407877,24.5660118766340,46.0670968831061,63.1772269464555];
%load transition probabilities
trans_probs = [0.500000000000000,0.0953909984666727,0.0936862992694146,0.104581942815911,0.0849463335437900,0.121394425904212;...
    0.102287865624095,0.500000000000000,0.0917752678830003,0.102558161984748,0.0842648904334395,0.119113814074718;0.101311261421621,...
    0.0948241762588505,0.500000000000000,0.101766148490962,0.0846386614453542,0.117459752383213;0.103803845526540,0.0960747358692304,...
    0.0942443957159167,0.500000000000000,0.0871948677986988,0.118682155089614;0.100744217514483,0.0920229185166392,0.0896338111548057,...
    0.0993593809463402,0.500000000000000,0.118239671867732;0.109138242310916,0.0989022503062738,0.0960571281191566,0.106954026694178,0.0889483525694758,0.500000000000000];
%load observation noise data
kappa =[0.327973912574545;0.303909349115727;0.282744198518673;0.257369355096232;0.239081601404447;0.217386355882733;0.199072758206112;0.183132887880788;0.165871243388662;...
    0.150781712435529;0.137445214975176;0.126283512235535;0.114402394802476;0.104466246018628;0.0947204546876257;0.0862050943518338;0.0782510877463125;0.0707492641810876;...
    0.0644826158612981;0.0585904141685921;0.0529404263834298;0.0480940839999611;0.0436761803436944;0.0398349470828149;0.0359906621644362];
kappa_s = ((10./log(10)).^2).*(kappa+0.1);
kappa =  repmat(kappa_s,1,(Csts.cl)^2);
%-------------------------------------------------------------------------%
%-------------------------------------------------------------------------%
%take care of the HMM states
switch Csts.cl
    case 6
        %do nothing
        change_tp = 0;
    case 5
        mStates(:,6)=[];
        covStates(:,:,6)=[];
        trans_probs(6,:) = [];trans_probs(:,6) = [];
        change_tp = 1;
    case 4
        mStates(:,6)=[];
        covStates(:,:,6)=[];
        trans_probs(6,:) = [];trans_probs(:,6) = [];
        mStates(:,1)=[];
        covStates(:,:,1)=[];
        trans_probs(1,:) = [];trans_probs(:,1) = [];
        change_tp = 1;
    case 3
        mStates(:,6)=[];
        covStates(:,:,6)=[];
        trans_probs(6,:) = [];trans_probs(:,6) = [];
        mStates(:,1)=[];
        covStates(:,:,1)=[];
        trans_probs(1,:) = [];trans_probs(:,1) = [];
        mStates(:,2)=[];
        covStates(:,:,2)=[];
        trans_probs(2,:) = [];trans_probs(:,2) = [];
        change_tp = 1;
    case 2
        mStates(:,6)=[];
        covStates(:,:,6)=[];
        trans_probs(6,:) = [];trans_probs(:,6) = [];
        mStates(:,1)=[];
        covStates(:,:,1)=[];
        trans_probs(1,:) = [];trans_probs(:,1) = [];
        mStates(:,2)=[];
        covStates(:,:,2)=[];
        trans_probs(2,:) = [];trans_probs(:,2) = [];
        mStates(:,2)=[];
        covStates(:,:,2)=[];
        trans_probs(2,:) = [];trans_probs(:,2) = [];
        change_tp = 1;
    otherwise
        error('The number of states you have selected is not permitted !');
end
if change_tp ==1
    for i=1:Csts.cl
        trans_probs(i,i)=0.5;
        trans_probs(i,union(1:i-1,i+1:Csts.cl)) = 0.5*trans_probs(i,union(1:i-1,i+1:Csts.cl))./sum(trans_probs(i,union(1:i-1,i+1:Csts.cl)));
    end
end
%-------------------------------------------------------------------------%
%Prepare the STFT
if Csts.ri
    ni=pow2(nextpow2(Csts.ti*fs*sqrt(0.5)));
else
    ni=round(Csts.ti*fs);    % frame increment in samples
end
tinc=ni/fs;          % true frame increment in time
no=round(Csts.of);            % integer overlap factor
nf=ni*no;           % fft length (size of analysis window)
w_synth=hann(nf+1)'; w_synth(end)=[];  % hamming window for synthesis
w=w_synth/sqrt(sum(w_synth(1:ni:nf).^2));
%-------------------------------------------------------------------------%
%Mel scale transformation matrices
[ThO_Tr,~]=v_filtbankm(25,nf,fs,[],[],'m'); %forward transformation matrix)
reco_mat = interpofiltbankm(25,nf,fs); %inverse transformation matrix (interpolation of the gain from mel bands to full STFT spectrum - see function below)
%-------------------------------------------------------------------------%
[DFTc,~]=v_enframe(input_speech,w,ni,'z'); %zero-padding the end of the signal to match the number of frames
C=v_rfft(DFTc,nf,2);
[nrows,ncols] = size(C);
gt_YP_full=(C.'.*conj(C.')./(ncols*sum(w.^2)));
gt_YP = 10.*log10(ThO_Tr*gt_YP_full);
gt_YP = max(gt_YP,max(gt_YP(:))+Csts.ef); % clip to 60 dB range avoid negative infinities
Energy = 10.^(gt_YP./10);
%-------------------------------------------------------------------------%
%various initialisations
K = Csts.cl; %Number of states in our HMM speech model
nfc = size(gt_YP,1);
nb_frames = size(gt_YP,2);
if Csts.mo ==0
    %UDU decomposition of the covariance of each state distribution
    U_state = zeros(nfc,nfc,K);
    D_state = zeros(nfc,nfc,K);
    for i=1:K
        [tmpU,tmpD] = udu(covStates(:,:,i));
        U_state(:,:,i) = tmpU;
        D_state(:,:,i) = tmpD;
    end
end
%init clean speech posterior mean and covariance with priors
M_speech = mStates;
if Csts.mo ==0
    U_speech = U_state;
    D_speech = D_state;
else
    Cov_speech = covStates;
end
%probs of each path
probs = log((1/(K)).*ones(K,1)); %initialise the probabilities
%our state space representation --> [gain;reverb_power_in_subbands;noise_power_in_subbands]
X = zeros(2*nfc+1,K);
X(1,:) = -12.*ones([1,K]);
X(2:nfc+1,:) = repmat((mean(gt_YP(:,1:12),2)),[1,K]); %initialise reverberation energy to the mean observed power of the first frames (same as noise)
X(nfc+2:2*nfc+1,:) = repmat(mean(gt_YP(:,1:12),2),[1,K]);%initialise noise level to the mean of the 1st few frames
%initialize the reverb parameters
alpha = 10.^((-6.*tinc)./[0.625814328889286,0.635814328889286,0.559669908460684,0.533597692539940,0.523597692539940,0.519657090507566,...
    0.498220803534608,0.488220803534608,0.470970193330601,0.460004032902958,0.445158336751369,0.436812074651546,...
    0.400699712568631,0.395830867067534,0.399005310050930,0.401204573100827,0.417815999144577,...
    0.417892069024374,0.419406546706839,0.422573783237992,0.417601620276530,0.408528394010926,...
    0.403943250672072,0.354372247720777,0.316876109784076])'; %average subbands T60 values from measurements on a variety of RIRs (inaccurate at low freqs)
f = (1-alpha)./(10.^(linspace(-0.2,0.8,25)')); %corresponds to DRR = [-2dB --> 8dB]
Rp = [10.*log10(alpha./(1-alpha))+1.5;10.*log10(f./(1-f))];
%Initialisation of some arrays used in the main body of the function
X_rev = X(:,1);
Speech_rev = mStates(:,1);
%Set up the time update covariance matrices
Qx=[1/350000,zeros([1,2*nfc]);...
    zeros([nfc,1]),diag(repmat(1/1500,[nfc,1])),zeros(nfc);...
    zeros([nfc,nfc+1]),diag(repmat(1/7550,[nfc,1]))];
Qr=[diag(repmat(1/1700,[nfc,1])),zeros(nfc);...
    zeros(nfc),diag(repmat(1/700,[nfc,1]))];
Qx = 15.*Qx + 1e-5*eye(2*nfc+1)*trace(Qx);
Qr = 15.*Qr + 1e-5*eye(2*nfc)*trace(Qr);
Covx = Qx.*1.5;
Covr = Qr.*15;
%Initialise the noise covariance matrix with the observation from the first few frames
% Covx(nfc+2:end,nfc+2:end) = cov(gt_YP(:,1:10).');
Covx(nfc+2:end,nfc+2:end) = Covx(nfc+2:end,nfc+2:end)+cov(gt_YP(:,1:10).'); % modified to prevent errors when initial frames are silent
if Csts.mo == 0 %UDU decomposition of the state space covariance matrix (SR-EKF implementation)
    Uq = eye(2*nfc+1);
    [tU,tD] = udu(Covx);
    Up = repmat(tU,[1,1,K]);
    Dp = repmat(tD,[1,1,K]);
    Um = zeros(2*nfc+1,2*nfc+1,K);
    Dm = Um;
else
    Cov = cell(K,1);
    for i=1:K
        Cov{i} = Covx;
    end
end
%matrices used in the computation of the prediction and update stages (vectorisation purposes)
pdm = [zeros(nfc,1),eye(nfc),zeros(nfc),eye(nfc),zeros(nfc,2*nfc);...
    zeros(nfc,2*nfc+1),eye(nfc),zeros(nfc,2*nfc);...
    ones(nfc,1),zeros(nfc,3*nfc),eye(nfc),eye(nfc);...
    zeros(nfc,3*nfc+1),eye(nfc),zeros(nfc)];
pdm2 = [zeros(nfc,1);ones(nfc,1);zeros(nfc,1);ones(nfc,1)];
pdm2_aug = repmat(pdm2,1,K);
pdm3 = [ones(nfc,1),zeros(nfc,2*nfc),eye(nfc);...
    zeros(nfc,1),eye(nfc),zeros(nfc,2*nfc);...
    zeros(nfc,nfc+1),eye(nfc),zeros(nfc)];
stat_mat = reshape(repmat(mStates,K,1),nfc,K^2);
%Declaration of Jacobians
Fx = eye(2*nfc+1);
Fu = zeros(2*nfc+1,nfc);
if Csts.mo ==0
    Hx = zeros(nfc,2*nfc+1);
else
    Hx = cell(K,K);
    Hu = cell(K,K);
end
%Initialise Gain
SpecGain = zeros(ncols,nb_frames+1); SpecGain(:,1) = 0.00001.*ones(ncols,1);
%%%%% extra stuff we need if we're after MMSE estimator of clean speech %%%%%
if Csts.sg == 3
    Xi = 0.00001.*ones(nfc,1);
    p0 = 0.5;
    pInf = 1;
    mu_mmse = 0.5; % shape parameter of the chi distribution for clean speech magnitude
    beta_mmse = 0.5; % compression factor
    gammaFactor = (gamma(mu_mmse+beta_mmse/2)./gamma(mu_mmse));
end
%-------------------------------------------------------------------------%
%Main loop, frame by frame processing
for idx=2:nb_frames+1
    
    %%%%% first do the prediction stage for each track %%%%%
    
    if Csts.mo ==0
        tmpX = X;
        for k = 1 : K %for each track
            tmpaug = [X(:,k);Rp;M_speech(:,k)]; %create the augmented state
            tmp = reshape(exp((pdm*tmpaug).*0.2302585093)+pdm2,[nfc,4]); %0.2302585093 = log(10)/10
            tmp_sum = tmp(:,1)./tmp(:,2) + tmp(:,3)./tmp(:,4);
            %compute the reverb power part of the output state
            tmpX(2:nfc+1,k) = 10.*log10(tmp_sum);
            %compute the jacobians
            Fx(2:nfc+1,1:nfc+1) = [(tmp(:,3)./tmp(:,4))./tmp_sum,diag((tmp(:,1)./tmp(:,2))./tmp_sum)];
            Fu(2:nfc+1,:) = diag(Fx(2:nfc+1,1));
            %compute the covariance matrix (SR-EKF)
            [Um(:,:,k),Dm(:,:,k)] = mwgs_factor([Uq,Fx*Up(:,:,k),Fu*U_speech(:,:,k)],diag([diag(Qx);diag(Dp(:,:,k));diag(D_speech(:,:,k))])); %Covariance matrix of the prediction stage in UDU form
        end
    else
        tmpaug = [X;repmat(Rp,1,K);M_speech];%create the augmented state
        tmp = exp((pdm*tmpaug).*0.2302585093)+pdm2_aug;
        tmp_sum1 = tmp(1:nfc,:)./tmp(nfc+1:2*nfc,:);
        tmp_sum2 = tmp(2*nfc+1:3*nfc,:)./tmp(3*nfc+1:end,:);
        tmp_sum = tmp_sum1 + tmp_sum2;
        %compute the reverb power part of the output state
        X(2:nfc+1,:) = 10.*log10(tmp_sum);
        %Jacobian comp
        Ftemp = [tmp_sum2./tmp_sum;tmp_sum1./tmp_sum];
        for k = 1 : K %for each track
            %compute the jacobians
            Fx(2:nfc+1,1:nfc+1) = [Ftemp(1:nfc,k),diag(Ftemp(nfc+1:end,k))];
            Fu(2:nfc+1,:) = diag(Ftemp(1:nfc,k));
            %compute the covariance matrix
            Cov{k} = Fu*Cov_speech(:,:,i)*Fu' + Fx*Cov{k}*Fx' + Qx;
        end
    end
    new_probs = repmat(probs,1,K) + trans_probs;
    
    %%%%% now the update stage with K^2 possibilities %%%%%
    
    %some init of various results we need to store
    err = zeros(nfc,K,K);
    Sk = zeros(nfc,nfc,K,K);
    lkl = zeros(K,K);
    if Csts.mo ==0
        vx = zeros(2*nfc+1,nfc,K,K);
        vu = zeros(nfc,nfc,K,K);
        for k1 = 1:K
            for k2 = 1:K
                %get mean of the augmented state
                m_tilde = [tmpX(:,k1);mStates(:,k2)];
                %compute jacobians
                tmp2 = reshape(exp((pdm3*m_tilde).*0.2302585093),[nfc,3]);
                tmp_sum2 = sum(tmp2,2);
                Hx(:,1) = tmp2(:,1)./tmp_sum2;
                Hx(:,2:nfc+1) = diag(tmp2(:,2)./tmp_sum2);
                Hx(:,nfc+2:2*nfc+1) = diag(tmp2(:,3)./tmp_sum2);
                Hu = diag(Hx(:,1));
                %compute predicted output and error
                zk = 10.*log10(tmp_sum2);
                err(:,k1,k2) = gt_YP(:,idx-1)-zk; %error between update stage and observed log-power
                R = diag(kappa_s+((10/log(10))^2).*log(1+2.*(tmp2(:,1).*tmp2(:,2)+tmp2(:,1).*tmp2(:,3)+tmp2(:,2).*tmp2(:,3))./tmp_sum2));
                %marginal on the output
                vx(:,:,k1,k2) = (Um(:,:,k1)')*Hx';
                vu(:,:,k1,k2) = (U_state(:,:,k2)')*Hu';
                [Usk,Dsk] = mwgs_factor([eye(nfc),vx(:,:,k1,k2)',vu(:,:,k1,k2)'],diag([diag(R);diag(Dm(:,:,k1));diag(D_state(:,:,k2))])); %Covariance matrix of the prediction stage in UDU form
                Sk(:,:,k1,k2) = Usk*Dsk*Usk'; %covariance matrix of the observation
                %compute the likelihood
                lkl(k1,k2) = v_gaussmixp(gt_YP(:,idx-1)',zk',Sk(:,:,k1,k2)); %v_gaussmixp returns log-probability
            end
        end
        
    else
        %get mean of the augmented state
        m_tilde = [repmat(X,1,K);stat_mat];
        tmp2 = exp((pdm3*m_tilde).*0.2302585093);
        tmp2_sum2 = tmp2(1:nfc,:) + tmp2(nfc+1:2*nfc,:) + tmp2(2*nfc+1:end,:);
        %predicted output
        zk = 10.*log10(tmp2_sum2);
        %observation noise
        R = kappa+((10/log(10))^2).*2.*(tmp2(1:nfc,:).*tmp2(nfc+1:2*nfc,:)+tmp2(1:nfc,:).*tmp2(2*nfc+1:end,:)+tmp2(nfc+1:2*nfc,:).*tmp2(2*nfc+1:end,:))./tmp2_sum2;
        %Jacob comp
        Htemp = tmp2./repmat(tmp2_sum2,3,1);
        for k1 = 1:K
            for k2 = 1:K
                %compute jacobians
                Hx{k1,k2} = [Htemp(1:nfc,K*(k2-1)+k1),diag(Htemp(nfc+1:2*nfc,K*(k2-1)+k1)),diag(Htemp(2*nfc+1:end,K*(k2-1)+k1))];
                Hu{k1,k2} = diag(Htemp(1:nfc,K*(k2-1)+k1));
                %compute predicted error
                err(:,k1,k2) = gt_YP(:,idx-1)-zk(:,K*(k2-1)+k1); %error between update stage and observed log-power
                %marginal on the output
                Sk(:,:,k1,k2) = Hx{k1,k2}*Cov{k1}*Hx{k1,k2}' + Hu{k1,k2}*covStates(:,:,k2)*Hu{k1,k2}' + diag(R(:,K*(k2-1)+k1));
                %compute the likelihood
                try
                    lkl(k1,k2) = v_gaussmixp(gt_YP(:,idx-1)',zk(:,K*(k2-1)+k1)',Sk(:,:,k1,k2)); %v_gaussmixp returns log-probability
                catch
                    error('Covariance matrix not positive definite - To avoid this, try a higher energy floor (e.g. algo_params.ef=-50) or running the algorithm in slow mode using algo_params.mo = 0');
                end
            end
        end
    end
    joint_lkl = new_probs + lkl;
    %now pick the best track arriving at each HMM state and rearrange
    [max_val,max_idx] = max(joint_lkl);
    probs = max_val';
    
    %Compute the posterior densities for the best tracks only
    if Csts.mo == 0
        for k = 1 : K
            %Compute posterior means
            K_n = (Um(:,:,max_idx(k))*Dm(:,:,max_idx(k))*vx(:,:,max_idx(k),k))/Sk(:,:,max_idx(k),k); %Kalman Gain for state space (Square root EKF)
            K_n_u = (U_state(:,:,k)*D_state(:,:,k)*vu(:,:,max_idx(k),k))/Sk(:,:,max_idx(k),k); %Kalman Gain for clean speech (Square root EKF)
            X(:,k) = tmpX(:,max_idx(k)) + K_n*err(:,max_idx(k),k);
            M_speech(:,k) = mStates(:,k) + K_n_u*err(:,max_idx(k),k);
            %Compute posterior covariances
            [B,Dp(:,:,k)] = udu(Dm(:,:,max_idx(k)) - (((Dm(:,:,max_idx(k))*vx(:,:,max_idx(k),k))/Sk(:,:,max_idx(k),k))*(vx(:,:,max_idx(k),k).')*Dm(:,:,max_idx(k)))); %UDU update posterior covariance matrix state space (SR-EKF)
            [Bs,D_speech(:,:,k)] = udu(D_state(:,:,k) - (((D_state(:,:,k)*vu(:,:,max_idx(k),k))/Sk(:,:,max_idx(k),k))*(vu(:,:,max_idx(k),k).')*D_state(:,:,k))); %UDU update posterior covariance matrix clean speech (SR-EKF)
            Up(:,:,k) = Um(:,:,max_idx(k))*B;
            U_speech(:,:,k) = U_state(:,:,k)*Bs;
        end
    else
        Cov_back = Cov;
        for k = 1 : K
            %Compute posterior means
            K_n = (Cov{max_idx(k)}*Hx{max_idx(k),k}')/Sk(:,:,max_idx(k),k); %Kalman Gain for state space (Square root EKF)
            K_n_u = (covStates(:,:,k)*Hu{max_idx(k),k}')/Sk(:,:,max_idx(k),k); %Kalman Gain for clean speech (Square root EKF)
            X(:,k) = X(:,max_idx(k)) + K_n*err(:,max_idx(k),k);
            M_speech(:,k) = mStates(:,k) + K_n_u*err(:,max_idx(k),k);
            %Compute posterior covariances
            Cov{k} = Cov_back{max_idx(k)} - K_n*Sk(:,:,max_idx(k),k)*K_n';
            Cov_speech(:,:,k) = covStates(:,:,k) - K_n_u*Sk(:,:,max_idx(k),k)*K_n_u';
        end
    end
    
    %get the weighted_sum state space
    weights = exp(max_val-max(max_val))';
    weights= weights/sum(weights);
    Weighted_X = sum(reshape(X,[2*nfc+1,K]).*repmat(weights',2*nfc+1,1),2);
    Weighted_Speech = sum(M_speech.*repmat(weights',nfc,1),2);
    if Csts.ds == 2 %If the user decides to do the enhancement using the weighted sum of the densities
        Weighted_cov = 0;
        Weighted_cov_speech = 0;
        if Csts.mo ==0
            for k=1:K
                Weighted_cov = Weighted_cov + weights(k).*...
                    (Up(:,:,k)*Dp(:,:,k)*(Up(:,:,k)')) + weights(k).*((X(:,k)-...
                    Weighted_X)*(X(:,k)-Weighted_X)');
                Weighted_cov_speech = Weighted_cov_speech + ...
                    weights(k).*(U_speech(:,:,k)*D_speech(:,:,k)*(U_speech(:,:,k)')) + ...
                    weights(k).*((M_speech(:,k)-Weighted_Speech)*(M_speech(:,k)-Weighted_Speech)');
            end
        else
            for k=1:K
                Weighted_cov = Weighted_cov + weights(k).*...
                    Cov{k} + weights(k).*((X(:,k)-...
                    Weighted_X)*(X(:,k)-Weighted_X)');
                Weighted_cov_speech = Weighted_cov_speech + ...
                    weights(k).*Cov_speech(:,:,k) + ...
                    weights(k).*((M_speech(:,k)-Weighted_Speech)*(M_speech(:,k)-Weighted_Speech)');
            end
        end
    end
    
    %%%%% Update the reverb parameters estimate %%%%%
    
    %model prediction stage : no movement/random walk
    Covr = Covr + Qr;
    %update stage : the observation is the new posterior on reverb power
    tmpaugpr = [X_rev;Rp;Speech_rev];
    tmppr = reshape(exp((pdm*tmpaugpr).*0.2302585093)+pdm2,[nfc,4]); %0.2302585093 = log(10)/10
    tmp_sumpr = tmppr(:,1)./tmppr(:,2) + tmppr(:,3)./tmppr(:,4);
    outrev = 10.*log10(tmp_sumpr);
    Gx = [diag((tmppr(:,1)./(tmppr(:,2).^2))./tmp_sumpr),diag((tmppr(:,3)./(tmppr(:,4).^2))./tmp_sumpr)];
    pred_err = Weighted_X(2:nfc+1)-outrev;
    RevSk = Gx*Covr*Gx' + trace(Covr).*eye(nfc)./5; %add some artificial observation noise as our model is approximate and to avoid big jumps in the reverb parameters as a result
    RevK = (Covr*(Gx'))/RevSk; %Kalman Gain
    Rp = Rp + RevK*pred_err;
    Covr = Covr - RevK*RevSk*(RevK');
    %prepare for next time frame
    X_rev = Weighted_X;
    Speech_rev = Weighted_Speech;
    
    %Transform instantaneous best powers for gain computation in the power domain (processing with minimum latency)
    if Csts.ds == 1
        [~,max_idx2] = max(max_val); %get the instantaneous best index
        if Csts.mo ==0
            GainRevNoise = exp((log(10)/10).*X(:,max_idx2) - 0.5.*diag(((log(10)/10)^2).*(Up(:,:,max_idx2)*...
                Dp(:,:,max_idx2)*Up(:,:,max_idx2)')));
            SpeechPost = GainRevNoise(1).*exp((log(10)/10).*M_speech(:,max_idx2) - 0.5.*diag(((log(10)/10)^2).*(U_speech(:,:,max_idx2)*...
                D_speech(:,:,max_idx2)*U_speech(:,:,max_idx2)')));
        else
            GainRevNoise = exp((log(10)/10).*X(:,max_idx2) - 0.5.*diag(((log(10)/10)^2).*Cov{k}));
            SpeechPost = GainRevNoise(1).*exp((log(10)/10).*M_speech(:,max_idx2) - 0.5.*diag(((log(10)/10)^2).*Cov_speech(:,:,k)));
        end
    else
        GainRevNoise = exp((log(10)/10).*Weighted_X - 0.5.*diag(((log(10)/10)^2).*Weighted_cov));
        SpeechPost = GainRevNoise(1).*exp((log(10)/10).*Weighted_Speech - 0.5.*diag(((log(10)/10)^2).*Weighted_cov_speech));
    end
    %compute the gain
    switch Csts.sg
        case 1
            %Wiener gain
            SpecGain(:,idx) = Csts.sc .* SpecGain(:,idx-1) + (1-Csts.sc) .* (reco_mat*(SpeechPost./(SpeechPost + Csts.os.*(GainRevNoise(2:nfc+1) + GainRevNoise(2+nfc:2*nfc+1)))));
        case 2
            %power spectral gain
            SpecGain(:,idx) = Csts.sc .* SpecGain(:,idx-1) + (1-Csts.sc) .* sqrt(reco_mat*(SpeechPost./(SpeechPost + Csts.os.*(GainRevNoise(2:nfc+1) + GainRevNoise(2+nfc:2*nfc+1)))));
        case 3
            %mmse estimate of clean speech
            Xi = Csts.sc .* Xi + (1-Csts.sc) .* (SpeechPost./(Csts.os.*(GainRevNoise(2:nfc+1) + GainRevNoise(2+nfc:2*nfc+1))));
            Gamma_kl = Energy(:,idx-1)./(Csts.os.*(GainRevNoise(2:nfc+1) + GainRevNoise(2+nfc:2*nfc+1)));
            nu_kl = (Gamma_kl.*Xi)./(mu_mmse+Xi);
            aHat0 = sqrt(Xi./(mu_mmse+Xi)) .* (gammaFactor).^(1/beta_mmse) .* (1./sqrt(Gamma_kl));
            SpecGain(:,idx) = reco_mat*((1./(1+nu_kl)).^p0 .* aHat0 + (nu_kl./(1+nu_kl)).^pInf .* Xi./(mu_mmse+Xi));
    end
    
end
%-------------------------------------------------------------------------%
FinalGain = max(SpecGain(:,2:end),Csts.sf);
%-------------------------reconstruct audio signal -----------------------%
ze_post_sub=(v_irfft((C.*FinalGain.').',nf).').*repmat(w,nrows,1);   % Inverse DFT and apply output window
enhanced_speech=zeros(ni*(nrows+no-1),no);                      % Allocate space for overlapped output speech
for i=1:no
    nm=nf*(1+floor((nrows-i)/no));  % Number of samples in this set
    enhanced_speech(1+(i-1)*ni:nm+(i-1)*ni,i)=reshape(ze_post_sub(i:no:nrows,:)',nm,1);
end
enhanced_speech=sum(enhanced_speech,2);
enhanced_speech=enhanced_speech(1:length(input_speech)); %make sure they're the same length
enhanced_speech = enhanced_speech.*10^((v_activlev(input_speech, fs, 'rd') - v_activlev(enhanced_speech, fs, 'rd'))/20); %normalise levels
if (fs_ori ~= 16000)
    enhanced_speech = resample(enhanced_speech,fs_ori,16000); %put back to original sampling frequency
end
end
%-------------------------------------------------------------------------%
%-------------------------------------------------------------------------%
%-------------------------------------------------------------------------%
%-------------------------------------------------------------------------%
%%%%% Functions needed to compute the main body of code %%%%%
%-------------------------------------------------------------------------%
%-------------------------------------------------------------------------%
%-------------------------------------------------------------------------%
%-------------------------------------------------------------------------%
function [u,d] = mwgs_factor(W,D) %needed for slow mode
% UD factorization using the Modified Weighted Gram-Schmidt method
% Reference : Catherine Thornton, 'Triangular Covariance
%			  Factorizations for Kalman Filtering', PhD Thesis, 1976.
[n,m] = size(W);
[k1,k2] = size(D);
if ((n>m)||(m~=k1)||(k1~=k2))
    error('!! Error !! - Check input matrices dimensions');
end
u = eye(n);
d = zeros(n);
w = W;  %copy to work on this one instead
for j=n:-1:1
    d(j,j) = w(j,:)*D*(w(j,:).');
    for i=1:j-1
        u(i,j) = (1/d(j,j)).*(w(i,:)*D*(w(j,:).'));
        w(i,:) = w(i,:) - u(i,j).*w(j,:);
    end
end
end
%-------------------------------------------------------------------------%
%%%%%% %%%%% %%%%% %%%%% %%%%% %%%%% %%%%% %%%%% %%%%%% %%%%% %%%%% %%%%% %%%%%
%-------------------------------------------------------------------------%
function [U,D]=udu(P) %needed for slow mode
% UDU Decomposition ---> P=U*D*U'
% Reference : Gerald J. Bierman, 'Factorization methods for discrete
% 			  sequential estimation', Mathematics in science and engineering,
% 			  Volume 128, 1977.
[~,n]=size(P);
for j=n:-1:2
    D(j,j)=P(j,j);
    alpha=1/D(j,j);
    for k=1:1:j-1
        beta=P(k,j);
        U(k,j)=alpha*beta;
        for i=1:1:k
            P(i,k)=P(i,k)-beta*U(i,j);
        end
    end
end
D(1,1)=P(1,1);
for i=1:1:n
    U(i,i)=1;
end
end
%-------------------------------------------------------------------------%
%%%%%% %%%%% %%%%% %%%%% %%%%% %%%%% %%%%% %%%%% %%%%%% %%%%% %%%%% %%%%% %%%%%
%-------------------------------------------------------------------------%
function [x]=interpofiltbankm(p,n,fs)
%INTERPOFILTBANKM determines interpolation matrix for a MEL to STFT bins transformation
%
%  VERY heavily inspired by FILTBANKM from Mike Brookes' v_voicebox MATLAB toolbox
%  (basically just changed the beginning to do the reverse operation,
%   the rest of the code is pretty much the same)
%
% Inputs:
%       p   number of filters in v_filterbank or the filter spacing in k-mel/bark/erb [ceil(4.6*log10(fs))]
%		n   length of fft
%		fs  sample rate in Hz
%
% Outputs:	x     a matrix containing the v_filterbank amplitudes
%                 size(x)=[p,1+floor(n/2)]
%

w='m';
wr ='m';
fh=0.5*fs; % max freq is the nyquist
fl=0;

f1=0;
nf=1+floor(n/2); % number of input frequency bins
df=fs/n;  % input frequency bin spacing
cf=f1+(0:nf)*df;  % input frequency bins

mflh=[fl fh];
mflh=v_frq2mel(mflh);       % convert frequency limits into mel
melrng=mflh*(-1:2:1)';          % mel/erb/... range
% fn2=floor(n/2);     % bin index of highest positive frequency (Nyquist if n is even)
melinc=melrng/(p+1);

fin0 = mflh(1)+(0:p+1)*melinc; % centre frequencies in mel/erb/... including dummy ends
fin0(2:end)=max(fin0(2:end),0); % only the first point can be negative
fin0 = v_mel2frq(fin0);

cf = [cf(1)-df,cf];
p = length(cf) - 2;
mb = cf;

% first sort out 2-sided input frequencies
fin=fin0;
%fin(end+1)=fin(end)+df; % add on a dummy point at the high end
fin=[-fin(end:-1:2) fin];
nfin=length(fin);  % length of extended input frequency list
nf = (nfin - 3)/2;

% now sort out the interleaving

fout=mb;  % output frequencies in Hz
lowex=any(w=='y')~=any(w=='Y');   % extend to 0 Hz
highex=any(w=='y') && (fout(end-1)<fin(end));  % extend at high end
if lowex
    fout=[0 0 fout(2:end)];
end
if highex
    fout=[fout(1:end-1) fin(end) fin(end)];
end
mfout=length(fout);
if any(w=='u') || any(w=='U')
    gout=fout(3:mfout)-fout(1:mfout-2);
    gout=2*(gout+(gout==0)).^(-1); % Gain of output triangles
else
    gout=ones(1,mfout-2);
end
if any(w=='u')
    gin=ones(1,nfin-2);
else
    gin=fin(3:nfin)-fin(1:nfin-2);
    gin=2*(gin+(gin==0)).^(-1); % Gain of input triangles
end
msk=fin(2:end-1)==0;
if lowex
    gin(msk)=2*gin(msk);  % double DC input to preserve its power
end
foutin=[fout fin];
nfall=length(foutin);
wleft=[0 fout(2:mfout)-fout(1:mfout-1) 0 fin(2:nfin)-fin(1:nfin-1)]; % left width
wright=[wleft(2:end) 0]; % right width
ffact=[0 gout 0 0 gin(1:min(nf,nfin-nf-2)) zeros(1,max(nfin-2*nf-2,0)) gin(nfin-nf-1:nfin-2) 0]; % gain of triangle posts
% ffact(wleft+wright==0)=0; % disable null width triangles shouldn't need this if all frequencies are distinct
[fall,ifall]=sort(foutin);
jfall=zeros(1,nfall);
infall=1:nfall;
jfall(ifall)=infall; % unsort->sort index
ffact(ifall([1:max(jfall(1),jfall(mfout+1))-2 min(jfall(mfout),jfall(nfall))+2:nfall]))=0;  % zap nodes that are much too small/big

nxto=cumsum(ifall<=mfout);
nxti=cumsum(ifall>mfout);
nxtr=min(nxti+1+mfout,nfall);  % next input node to the right of each value (or nfall if none)
nxtr(ifall>mfout)=1+nxto(ifall>mfout); % next post to the right of opposite type (unsorted indexes)
nxtr=nxtr(jfall);  % next post to the right of opposite type (converted to unsorted indices) or if none: nfall/(mfout+1)
% each triangle is "attached" to the node at its extreme right end
% the general result for integrating the product of two trapesiums with
% heights (a,b) and (c,d) over a width x is (ad+bc+2bd+2ac)*w/6
%
% integrate product of lower triangles
msk0=(ffact>0);
msk=msk0 & (ffact(nxtr)>0); % select appropriate triangle pairs (unsorted indices)
ix1=infall(msk); % unsorted indices of leftmost post of pair
jx1=nxtr(msk);  % unsorted indices of rightmost post of pair
vfgx=foutin(ix1)-foutin(jx1-1); % length of right triangle to the left of the left post
yx=min(wleft(ix1),vfgx); % integration length
wx1=ffact(ix1).*ffact(jx1).*yx.*(wleft(ix1).*vfgx-yx.*(0.5*(wleft(ix1)+vfgx)-yx/3))./(wleft(ix1).*wleft(jx1)+(yx==0));
% integrate product of upper triangles
nxtu=max([nxtr(2:end)-1 0],1);
msk=msk0 & (ffact(nxtu)>0);
ix2=infall(msk); % unsorted indices of leftmost post of pair
jx2=nxtu(msk);  % unsorted indices of rightmost post of pair
vfgx=foutin(ix2+1)-foutin(jx2); % length of left triangle to the right of the right post
yx=min(wright(ix2),vfgx); % integration length
yx(foutin(jx2+1)<foutin(ix2+1))=0; % zap invalid triangles
wx2=ffact(ix2).*ffact(jx2).*yx.^2.*((0.5*(wright(jx2)-vfgx)+yx/3))./(wright(ix2).*wright(jx2)+(yx==0));
% integrate lower triangle and upper triangle that ends to its right
nxtu=max(nxtr-1,1);
msk=msk0 & (ffact(nxtu)>0);
ix3=infall(msk); % unsorted indices of leftmost post of pair
jx3=nxtu(msk);  % unsorted indices of rightmost post of pair
vfgx=foutin(ix3)-foutin(jx3); % length of upper triangle to the left of the lower post
yx=min(wleft(ix3),vfgx); % integration length
yx(foutin(jx3+1)<foutin(ix3))=0; % zap invalid triangles
wx3=ffact(ix3).*ffact(jx3).*yx.*(wleft(ix3).*(wright(jx3)-vfgx)+yx.*(0.5*(wleft(ix3)-wright(jx3)+vfgx)-yx/3))./(wleft(ix3).*wright(jx3)+(yx==0));
% integrate upper triangle and lower triangle that starts to its right
nxtu=[nxtr(2:end) 1];
msk=msk0 & (ffact(nxtu)>0);
ix4=infall(msk); % unsorted indices of leftmost post of pair
jx4=nxtu(msk);  % unsorted indices of rightmost post of pair
vfgx=foutin(ix4+1)-foutin(jx4-1); % length of upper triangle to the left of the lower post
yx=min(wright(ix4),vfgx); % integration length
wx4=ffact(ix4).*ffact(jx4).*yx.^2.*(0.5*vfgx-yx/3)./(wright(ix4).*wleft(jx4)+(yx==0));

% now create the matrix

iox=sort([ix1 ix2 ix3 ix4;jx1 jx2 jx3 jx4]);
msk=iox(2,:)<=(nfall+mfout)/2;
iox(2,msk)=(nfall+mfout+1)-iox(2,msk);  % convert negative frequencies to positive
if highex
    iox(1,iox(1,:)==mfout-1)=mfout-2; % merge highest two output nodes
end
if lowex
    iox(1,iox(1,:)==2)=3; % merge lowest two output nodes
end

x=sparse(iox(1,:)-1-lowex,max(iox(2,:)-nfall+nf+1,1),[wx1 wx2 wx3 wx4],p,nf);

end